From 59c42f6ccc0d1817283f1cc5f32bee3c36705991 Mon Sep 17 00:00:00 2001 From: Brian Semrau Date: Fri, 23 Feb 2018 14:02:52 -0500 Subject: [PATCH] Fixed driving/input issues, improved intake control --- src/org/usfirst/frc/team4737/lib/Gamepad.java | 14 +++--- src/org/usfirst/frc/team4737/robot/OI.java | 7 ++- src/org/usfirst/frc/team4737/robot/Robot.java | 2 +- .../commands/drivetrain/TeleopTankDrive.java | 4 +- .../robot/commands/intake/ControlIntake.java | 5 ++- .../robot/commands/intake/TwistIntake.java | 43 +++++++++++++++++++ .../team4737/robot/subsystems/Drivetrain.java | 4 +- .../frc/team4737/robot/subsystems/Intake.java | 18 ++++---- 8 files changed, 76 insertions(+), 21 deletions(-) create mode 100644 src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java diff --git a/src/org/usfirst/frc/team4737/lib/Gamepad.java b/src/org/usfirst/frc/team4737/lib/Gamepad.java index 2111744..316f67a 100644 --- a/src/org/usfirst/frc/team4737/lib/Gamepad.java +++ b/src/org/usfirst/frc/team4737/lib/Gamepad.java @@ -94,7 +94,7 @@ private double scale(double val, double valLow, double valHigh, double newLow, d public class DPad { - private class DPadButton extends Button { + public class DPadButton extends Button { private final DPad dpad; private final int degree; @@ -121,7 +121,7 @@ public String getNameID() { } - private class DPadAxis extends Axis { + public class DPadAxis extends Axis { private DPadButton[] negative; private DPadButton[] positive; @@ -195,6 +195,8 @@ public DPad(Gamepad gamepad, String name, int id) { new DPadButton[] { RIGHT, DOWN_RIGHT, UP_RIGHT }); Y = new DPadAxis(gamepad, name + "_Y", new DPadButton[] { DOWN, DOWN_LEFT, DOWN_RIGHT }, new DPadButton[] { UP, UP_LEFT, UP_RIGHT }); + + gamepad.registerDPad(this, name); } /** @@ -294,7 +296,7 @@ public String getName() { * @return Returns a button mapped with the given name */ public Button getButton(String name) { - return buttonMap.get(name); + return buttonMap.get(name.toLowerCase()); } /** @@ -306,7 +308,7 @@ public Button getButton(String name) { * @return Returns an axis mapped with the given name */ public Axis getAxis(String name) { - return axisMap.get(name); + return axisMap.get(name.toLowerCase()); } /** @@ -319,7 +321,7 @@ public Axis getAxis(String name) { * @return Returns a thumbstick mapped with the given name */ public Thumbstick getThumbstick(String name) { - return thumbstickMap.get(name); + return thumbstickMap.get(name.toLowerCase()); } /** @@ -331,7 +333,7 @@ public Thumbstick getThumbstick(String name) { * @return Returns a button mapped with the given name */ public DPad getDPad(String name) { - return dpadMap.get(name); + return dpadMap.get(name.toLowerCase()); } } diff --git a/src/org/usfirst/frc/team4737/robot/OI.java b/src/org/usfirst/frc/team4737/robot/OI.java index 6d4a617..0228e56 100644 --- a/src/org/usfirst/frc/team4737/robot/OI.java +++ b/src/org/usfirst/frc/team4737/robot/OI.java @@ -14,6 +14,7 @@ import org.usfirst.frc.team4737.robot.commands.intake.ControlIntake; import org.usfirst.frc.team4737.robot.commands.intake.ReverseIntake; import org.usfirst.frc.team4737.robot.commands.intake.RunIntake; +import org.usfirst.frc.team4737.robot.commands.intake.TwistIntake; import edu.wpi.first.wpilibj.buttons.Trigger; @@ -61,7 +62,8 @@ public OI() { new Trigger() { @Override public boolean get() { - return operator.getAxis("LT").get() != 0 || operator.getAxis("RT").get() != 0; + return operator.getAxis("LT").get() != 0 || operator.getAxis("RT").get() != 0 + || operator.getAxis("RS_X").get() != 0; } }.whileActive(new ControlIntake()); @@ -80,6 +82,9 @@ public boolean get() { return operator.getAxis("LS_Y").get() != 0; } }.whileActive(new ControlElevator()); + + // operator.getDPad("DPAD").LEFT.whileActive(new TwistIntake(1)); + // operator.getDPad("DPAD").RIGHT.whileActive(new TwistIntake(-1)); } diff --git a/src/org/usfirst/frc/team4737/robot/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java index 09fa976..daaa0c9 100644 --- a/src/org/usfirst/frc/team4737/robot/Robot.java +++ b/src/org/usfirst/frc/team4737/robot/Robot.java @@ -28,11 +28,11 @@ */ public class Robot extends TimedRobot { - public static final OI OI = new OI(); public static final Drivetrain DRIVETRAIN = new Drivetrain(); public static final Elevator ELEVATOR = new Elevator(); public static final Intake INTAKE = new Intake(); public static final Climber CLIMBER = new Climber(); + public static final OI OI = new OI(); Command m_autonomousCommand; SendableChooser m_chooser = new SendableChooser<>(); diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java index cd1c975..990c8ac 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java @@ -22,8 +22,8 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.DRIVETRAIN.tankDrive( - Robot.OI.driver.getThumbstick("LS").Y.get(), - Robot.OI.driver.getThumbstick("RS").Y.get()); + -Robot.OI.driver.getThumbstick("LS").Y.get(), + -Robot.OI.driver.getThumbstick("RS").Y.get()); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java index 863cf79..7d4e56c 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java +++ b/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java @@ -19,7 +19,9 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.INTAKE.setSpeed(-Robot.OI.operator.getAxis("LT").get() + Robot.OI.operator.getAxis("RT").get()); + double speed = -Robot.OI.operator.getAxis("LT").get() + Robot.OI.operator.getAxis("RT").get(); + double twist = Robot.OI.operator.getAxis("RS_X").get(); + Robot.INTAKE.setLRSpeed(speed - twist, speed + twist); } // Make this return true when this Command no longer needs to run execute() @@ -36,4 +38,5 @@ protected void end() { protected void interrupted() { Robot.INTAKE.setSpeed(0); } + } diff --git a/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java new file mode 100644 index 0000000..3bcd93c --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java @@ -0,0 +1,43 @@ +package org.usfirst.frc.team4737.robot.commands.intake; + +import org.usfirst.frc.team4737.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class TwistIntake extends Command { + + private double speed; + + public TwistIntake(double speed) { + requires(Robot.INTAKE); + + this.speed = speed; + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.INTAKE.setLRSpeed(speed, -speed); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } + +} diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java index d156ece..297e60d 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java @@ -30,8 +30,8 @@ public Drivetrain() { leftBackSlave.follow(leftFrontMaster); rightBackSlave.follow(rightFrontMaster); - leftFrontMaster.configOpenloopRamp(0.5, 30); - rightFrontMaster.configOpenloopRamp(0.5, 30); + leftFrontMaster.configOpenloopRamp(0.25, 30); + rightFrontMaster.configOpenloopRamp(0.25, 30); drive = new DifferentialDrive(leftFrontMaster, rightFrontMaster); } diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java index d63a5cd..1e3d75a 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java @@ -15,15 +15,12 @@ public class Intake extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - private WPI_TalonSRX leftMotorMaster; - private WPI_TalonSRX rightMotorSlave; + private WPI_TalonSRX leftMotor; + private WPI_TalonSRX rightMotor; public Intake() { - leftMotorMaster = new WPI_TalonSRX(RobotMap.INTAKE_LEFT); - rightMotorSlave = new WPI_TalonSRX(RobotMap.INTAKE_RIGHT); - - rightMotorSlave.setInverted(true); - rightMotorSlave.follow(leftMotorMaster); + leftMotor = new WPI_TalonSRX(RobotMap.INTAKE_LEFT); + rightMotor = new WPI_TalonSRX(RobotMap.INTAKE_RIGHT); } public void initDefaultCommand() { @@ -37,7 +34,12 @@ public void initDefaultCommand() { * ranges from -1 to 1. */ public void setSpeed(double speed) { - leftMotorMaster.set(speed); + setLRSpeed(speed, speed); + } + + public void setLRSpeed(double left, double right) { + leftMotor.set(left); + rightMotor.set(right); } }