Modified controls and elevator operation

src/org/usfirst/frc/team4737/lib/F310Gamepad.java

@@ -62,13 +62,17 @@ public F310Gamepad(int usbPort) {
 		R3 = new GamepadButton(this, "R3", 10);
 		registerButton(R3, "RS");
 
-		LS = new Thumbstick(this, "LS", 0, 1);
-		RS = new Thumbstick(this, "RS", 4, 5);
+		LS = new Thumbstick(this, "LS", 0, 1, false, true);
+		LS.X.setDeadzone(0.1);
+		LS.Y.setDeadzone(0.1);
+		RS = new Thumbstick(this, "RS", 4, 5, false, true);
+		RS.X.setDeadzone(0.1);
+		RS.Y.setDeadzone(0.1);
 
 //		LT = new LogitechGamepadTriggerAxis(this, "LT", 2, true);
 //		RT = new LogitechGamepadTriggerAxis(this, "RT", 3, false);
-		LT = new GamepadAxis(this, "LT", 2);
-		RT = new GamepadAxis(this, "RT", 3);
+		LT = new GamepadAxis(this, "LT", 2, false);
+		RT = new GamepadAxis(this, "RT", 3, false);
 
 		DPAD = new DPad(this, "DPAD", 0);
 	}

src/org/usfirst/frc/team4737/lib/Gamepad.java

@@ -55,10 +55,13 @@ public class GamepadAxis extends Axis {
 		private final int axis;
 
 		private double deadzone;
+
+		private boolean inverted;
 
-		public GamepadAxis(Gamepad gamepad, String name, int axis) {
+		public GamepadAxis(Gamepad gamepad, String name, int axis, boolean inverted) {
 			super(gamepad, name);
 			this.axis = axis;
+			this.inverted = inverted;
 
 			gamepad.registerAxis(this, name);
 		}
@@ -75,7 +78,7 @@ public double getRaw() {
 
 		@Override
 		public double get() {
-			double raw = getRaw();
+			double raw = inverted ? -getRaw() : getRaw();
 			if (raw < -deadzone) {
 				return scale(raw, -1, -deadzone, -1, 0);
 			} else if (raw > deadzone) {
@@ -223,9 +226,9 @@ public class Thumbstick {
 
 		private String name;
 
-		public Thumbstick(Gamepad gamepad, String name, int axisX, int axisY) {
-			X = new GamepadAxis(gamepad, name + "_X", axisX);
-			Y = new GamepadAxis(gamepad, name + "_Y", axisY);
+		public Thumbstick(Gamepad gamepad, String name, int axisX, int axisY, boolean invertX, boolean invertY) {
+			X = new GamepadAxis(gamepad, name + "_X", axisX, invertX);
+			Y = new GamepadAxis(gamepad, name + "_Y", axisY, invertY);
 
 			this.name = name;
 

src/org/usfirst/frc/team4737/lib/XboxController.java

@@ -37,11 +37,15 @@ public XboxController(int usbPort) {
 		R3 = new GamepadButton(this, "R3", 10);
 		registerButton(R3, "RS");
 
-		LS = new Thumbstick(this, "LS", 0, 1);
-		RS = new Thumbstick(this, "RS", 4, 5);
-
-		LT = new GamepadAxis(this, "LT", 2);
-		RT = new GamepadAxis(this, "RT", 3);
+		LS = new Thumbstick(this, "LS", 0, 1, false, false);
+		LS.X.setDeadzone(0.1);
+		LS.Y.setDeadzone(0.1);
+		RS = new Thumbstick(this, "RS", 4, 5, false, false);
+		RS.X.setDeadzone(0.1);
+		RS.Y.setDeadzone(0.1);
+
+		LT = new GamepadAxis(this, "LT", 2, false);
+		RT = new GamepadAxis(this, "RT", 3, false);
 
 		DPAD = new DPad(this, "DPAD", 0);
 	}

src/org/usfirst/frc/team4737/robot/OI.java

@@ -8,13 +8,12 @@
 package org.usfirst.frc.team4737.robot;
 
 import org.usfirst.frc.team4737.lib.Gamepad;
+import org.usfirst.frc.team4737.lib.XboxController;
 import org.usfirst.frc.team4737.lib.F310Gamepad;
-import org.usfirst.frc.team4737.robot.commands.drivetrain.TeleopTankDrive;
+import org.usfirst.frc.team4737.robot.commands.drivetrain.TeleopRacingDrive;
 import org.usfirst.frc.team4737.robot.commands.elevator.ControlElevator;
+import org.usfirst.frc.team4737.robot.commands.elevator.RelaxElevator;
 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;
 
@@ -55,12 +54,11 @@ public class OI {
 	public Gamepad operator;
 
 	public OI() {
-		driver = new F310Gamepad(0);
+		driver = new XboxController(0);
 		operator = new F310Gamepad(1);
 
 		// User override to take control of the intake
 		new Trigger() {
-			@Override
 			public boolean get() {
 				return operator.getAxis("LT").get() != 0 || operator.getAxis("RT").get() != 0
 						|| operator.getAxis("RS_X").get() != 0;
@@ -69,20 +67,21 @@ public boolean get() {
 
 		// User override to take control of driving
 		new Trigger() {
-			@Override
 			public boolean get() {
 				return driver.getThumbstick("LS").Y.get() != 0 || driver.getThumbstick("RS").Y.get() != 0;
 			}
-		}.whileActive(new TeleopTankDrive());
+		}.whileActive(new TeleopRacingDrive());
 
 		// User override to take control of the elevator
 		new Trigger() {
-			@Override
 			public boolean get() {
 				return operator.getAxis("LS_Y").get() != 0;
 			}
 		}.whileActive(new ControlElevator());
 
+		// Allow operator to instantly relax elevator in case of motor overheat
+		operator.getButton("Y").whileHeld(new RelaxElevator());
+
 		//		operator.getDPad("DPAD").LEFT.whileActive(new TwistIntake(1));
 		//		operator.getDPad("DPAD").RIGHT.whileActive(new TwistIntake(-1));
 

src/org/usfirst/frc/team4737/robot/RobotMap.java

@@ -15,6 +15,10 @@
  */
 public class RobotMap {
 
+	// ################
+	// TalonSRX CAN IDs
+	// ################
+
 	// Drivetrain
 	public static final int DRIVE_LEFT_MASTER = 11;
 	public static final int DRIVE_LEFT_SLAVE = 12;
@@ -28,16 +32,18 @@ public class RobotMap {
 	// Elevator
 	public static final int ELEVATOR_MOTOR = 17;
 
+	// #########
+	// Constants
+	// #########
+
 	/**
-	 * The stall current of the elevator when fully extended
-	 */
-	public static final double ELEVATOR_TOPSTALL_AMPS = 40; // TODO measure
-	/**
-	 * The power (-1 to 1) to give to the elevator for it to hold position with a
-	 * POWER CUBE
+	 * The voltage to give to the elevator for it to hold constant position with a
+	 * cube
 	 */
-	public static final double ELEVATOR_HOLD_PCT = 0; // TODO measure
-
-	public static final double ELEVATOR_HOLD_TIME = 3;
+	public static final double ELEVATOR_HOLD_V = 1; // TODO measure
+	public static final double ELEVATOR_HOLD_TIME = 4; // TODO tune
+	public static final double ELEVATOR_MAX_DOWN_SPEED = -0.6; // TODO tune
+
+	public static final double DRIVE_SLOW_SCALE = 0.5;
 
 }

src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java

@@ -2,38 +2,21 @@
 
 import org.usfirst.frc.team4737.robot.Robot;
 
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.InstantCommand;
 
 /**
  *
  */
-public class RelaxDrivetrain extends Command {
+public class RelaxDrivetrain extends InstantCommand {
 
-    public RelaxDrivetrain() {
-        requires(Robot.DRIVETRAIN);
-    }
+	public RelaxDrivetrain() {
+		requires(Robot.DRIVETRAIN);
+		this.setRunWhenDisabled(true);
+	}
 
-    // Called just before this Command runs the first time
-    protected void initialize() {
-    	Robot.DRIVETRAIN.setCoastMode();
-    }
+	// Called just before this Command runs the first time
+	protected void initialize() {
+		Robot.DRIVETRAIN.setCoastMode();
+	}
 
-    // Called repeatedly when this Command is scheduled to run
-    protected void execute() {
-    }
-
-    // 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() {
-    }
-
 }

src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java

@@ -0,0 +1,46 @@
+package org.usfirst.frc.team4737.robot.commands.drivetrain;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class TeleopRacingDrive extends Command {
+
+	public TeleopRacingDrive() {
+		requires(Robot.DRIVETRAIN);
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+		Robot.DRIVETRAIN.setBrakeMode();
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		boolean slow = Robot.OI.driver.getButton("LB").get();
+		double throttle = (Robot.OI.driver.getAxis("RT").get() - Robot.OI.driver.getAxis("LT").get())
+				* (slow ? RobotMap.DRIVE_SLOW_SCALE : 1);
+		double steer = Robot.OI.driver.getThumbstick("LS").X.get() * (slow ? RobotMap.DRIVE_SLOW_SCALE : 1);
+
+		Robot.DRIVETRAIN.arcadeDrive(throttle, steer);
+	}
+
+	// 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() {
+	}
+
+}

src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java

@@ -9,35 +9,36 @@
  */
 public class TeleopTankDrive extends Command {
 
-    public TeleopTankDrive() {
-        // Use requires() here to declare subsystem dependencies
-        requires(Robot.DRIVETRAIN);
-    }
-
-    // Called just before this Command runs the first time
-    protected void initialize() {
-    	Robot.DRIVETRAIN.setBrakeMode();
-    }
-
-    // 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());
-    }
-
-    // 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() {
-    }
-
+	public TeleopTankDrive() {
+		// Use requires() here to declare subsystem dependencies
+		requires(Robot.DRIVETRAIN);
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+		Robot.DRIVETRAIN.setBrakeMode();
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		boolean slow = Robot.OI.driver.getButton("RB").get();
+		Robot.DRIVETRAIN.tankDrive(
+				-Robot.OI.driver.getThumbstick("LS").Y.get() * (slow ? 0.6 : 1),
+				-Robot.OI.driver.getThumbstick("RS").Y.get() * (slow ? 0.6 : 1));
+	}
+
+	// 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() {
+	}
+
 }

src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java

@@ -4,7 +4,6 @@
 import org.usfirst.frc.team4737.robot.RobotMap;
 
 import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  *
@@ -17,6 +16,7 @@ public ControlElevator() {
 
 	// Called just before this Command runs the first time
 	protected void initialize() {
+		Robot.ELEVATOR.setBrakeMode();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
@@ -25,19 +25,27 @@ protected void execute() {
 		double input = Robot.OI.operator.getAxis("LS_Y").get();
 
 		// Check if we're hitting the top
-		if (input > 0 && Robot.ELEVATOR.getMotorCurrent() > RobotMap.ELEVATOR_TOPSTALL_AMPS) {
-			input = 0;
-		}
-
-		Robot.ELEVATOR.setSpeed(input + RobotMap.ELEVATOR_HOLD_PCT);
+		//		if (input > 0 && Robot.ELEVATOR.isAtTop()) {
+		//			input = 0;
+		//		}
 
-		SmartDashboard.putNumber("elevator_input", input);
+		// Make sure we don't go down too fast
+		if (input < RobotMap.ELEVATOR_MAX_DOWN_SPEED) {
+			input = RobotMap.ELEVATOR_MAX_DOWN_SPEED;
+		}
 
 		// Start hold command if no input
 		if (input == 0) {
-			new HoldElevator().start();
-			this.cancel();
+			if (this.isRunning()) {
+				new HoldElevator().start();
+				this.cancel();
+			}
+			return;
 		}
+
+		// Set elevator speed
+		Robot.ELEVATOR.setSpeed(input);
+
 	}
 
 	// Make this return true when this Command no longer needs to run execute()