Added elevator/intake control

Also fixed tank drive, changed user inputs

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

@@ -28,7 +28,7 @@ public String getNameID() {
 
 	}
 
-	private abstract class Axis {
+	public abstract class Axis {
 
 		protected final Gamepad gamepad;
 

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

@@ -9,9 +9,13 @@
 
 import org.usfirst.frc.team4737.lib.Gamepad;
 import org.usfirst.frc.team4737.lib.LogitechGamepad;
+import org.usfirst.frc.team4737.robot.commands.ControlElevator;
+import org.usfirst.frc.team4737.robot.commands.ControlIntake;
 import org.usfirst.frc.team4737.robot.commands.ReverseIntake;
 import org.usfirst.frc.team4737.robot.commands.RunIntake;
+import org.usfirst.frc.team4737.robot.commands.TeleopTankDrive;
 
+import edu.wpi.first.wpilibj.buttons.Trigger;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -45,15 +49,38 @@ public class OI {
 	// Start the command when the button is released and let it run the command
 	// until it is finished as determined by it's isFinished method.
 	// button.whenReleased(new ExampleCommand());
-
-	public Gamepad controller;
-
+
+	public Gamepad driver;
+	public Gamepad operator;
+
 	public OI() {
-		controller = new LogitechGamepad(0);
-
-		controller.getButton("A").whileHeld(new RunIntake());
-		controller.getButton("B").whileHeld(new ReverseIntake());
-
+		driver = new LogitechGamepad(0);
+		operator = new LogitechGamepad(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;
+			}
+		}.whileActive(new ControlIntake());
+
+		// 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());
+
+		// User override to take control of the elevator
+		new Trigger() {
+			@Override
+			public boolean get() {
+				return operator.getAxis("LS_Y").get() != 0;
+			}
+		}.whileActive(new ControlElevator());
+
 	}
-	
+
 }

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

@@ -37,8 +37,8 @@ public class Robot extends TimedRobot {
 	SendableChooser<Command> m_chooser = new SendableChooser<>();
 
 	/**
-	 * This function is run when the robot is first started up and should be
-	 * used for any initialization code.
+	 * This function is run when the robot is first started up and should be used
+	 * for any initialization code.
 	 */
 	@Override
 	public void robotInit() {
@@ -48,9 +48,9 @@ public void robotInit() {
 	}
 
 	/**
-	 * This function is called once each time the robot enters Disabled mode.
-	 * You can use it to reset any subsystem information you want to clear when
-	 * the robot is disabled.
+	 * This function is called once each time the robot enters Disabled mode. You
+	 * can use it to reset any subsystem information you want to clear when the
+	 * robot is disabled.
 	 */
 	@Override
 	public void disabledInit() {
@@ -64,25 +64,25 @@ public void disabledPeriodic() {
 
 	/**
 	 * This autonomous (along with the chooser code above) shows how to select
-	 * between different autonomous modes using the dashboard. The sendable
-	 * chooser code works with the Java SmartDashboard. If you prefer the
-	 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
-	 * getString code to get the auto name from the text box below the Gyro
+	 * between different autonomous modes using the dashboard. The sendable chooser
+	 * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+	 * remove all of the chooser code and uncomment the getString code to get the
+	 * auto name from the text box below the Gyro
 	 *
 	 * <p>
 	 * You can add additional auto modes by adding additional commands to the
-	 * chooser code above (like the commented example) or additional comparisons
-	 * to the switch structure below with additional strings & commands.
+	 * chooser code above (like the commented example) or additional comparisons to
+	 * the switch structure below with additional strings & commands.
 	 */
 	@Override
 	public void autonomousInit() {
 		m_autonomousCommand = m_chooser.getSelected();
 
 		/*
-		 * String autoSelected = SmartDashboard.getString("Auto Selector",
-		 * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
-		 * = new MyAutoCommand(); break; case "Default Auto": default:
-		 * autonomousCommand = new ExampleCommand(); break; }
+		 * String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
+		 * switch(autoSelected) { case "My Auto": autonomousCommand = new
+		 * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
+		 * ExampleCommand(); break; }
 		 */
 
 		// schedule the autonomous command (example)
@@ -124,4 +124,5 @@ public void teleopPeriodic() {
 	@Override
 	public void testPeriodic() {
 	}
+
 }

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

@@ -26,11 +26,18 @@ public class RobotMap {
 	public static final int INTAKE_RIGHT = 16;
 
 	// Elevator
-	public static final int ELEVATOR_MOTOR_A = 17;
-	public static final int ELEVATOR_MOTOR_B = 18;
+	public static final int ELEVATOR_MOTOR = 17;
+
+	/**
+	 * 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
+	 */
+	public static final double ELEVATOR_HOLD_PCT = 0; // TODO measure
+
+	public static final double ELEVATOR_HOLD_TIME = 3;
 
-	// If you are using multiple modules, make sure to define both the port
-	// number and the module. For example you with a rangefinder:
-	// public static int rangefinderPort = 1;
-	// public static int rangefinderModule = 1;
 }

src/org/usfirst/frc/team4737/robot/commands/BuzzController.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc.team4737.robot.commands;
+
+import org.usfirst.frc.team4737.lib.Gamepad;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ *
+ */
+public class BuzzController extends TimedCommand {
+
+	private double strength;
+	private Gamepad controller;
+
+	public BuzzController(double timeout, double strength, Gamepad controller) {
+		super(timeout);
+		// requires(none);
+		this.strength = strength;
+		this.controller = controller;
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+		controller.setRumble(strength);
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+	}
+
+	// Called once after timeout
+	protected void end() {
+		controller.setRumble(0);
+	}
+
+	// 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/ControlElevator.java

@@ -0,0 +1,57 @@
+package org.usfirst.frc.team4737.robot.commands;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class ControlElevator extends Command {
+
+	public ControlElevator() {
+		requires(Robot.ELEVATOR);
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		// Control elevator movement, holding it in place by default
+		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);
+
+		SmartDashboard.putNumber("elevator_input", input);
+
+		// Start hold command if no input
+		if (input == 0) {
+			new HoldElevator().start();
+			this.cancel();
+		}
+	}
+
+	// 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/ControlIntake.java

@@ -0,0 +1,39 @@
+package org.usfirst.frc.team4737.robot.commands;
+
+import org.usfirst.frc.team4737.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ControlIntake extends Command {
+
+	public ControlIntake() {
+		requires(Robot.INTAKE);
+	}
+
+	// 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.setSpeed(-Robot.OI.operator.getAxis("LT").get() + Robot.OI.operator.getAxis("RT").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() {
+		Robot.INTAKE.setSpeed(0);
+	}
+}

src/org/usfirst/frc/team4737/robot/commands/HoldElevator.java

@@ -0,0 +1,42 @@
+package org.usfirst.frc.team4737.robot.commands;
+
+import org.usfirst.frc.team4737.robot.Robot;
+import org.usfirst.frc.team4737.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ *
+ */
+public class HoldElevator extends TimedCommand {
+
+	public HoldElevator() {
+		super(RobotMap.ELEVATOR_HOLD_TIME);
+		requires(Robot.ELEVATOR);
+	}
+
+	// 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.ELEVATOR.setSpeed(RobotMap.ELEVATOR_HOLD_PCT);
+
+		// If one second remaining, buzz controller
+		if (RobotMap.ELEVATOR_HOLD_TIME - this.timeSinceInitialized() < 1.0) {
+			new BuzzController(1, 0.5, Robot.OI.operator).start();
+		}
+	}
+
+	// Called once after timeout
+	protected void end() {
+		new RelaxElevator().start();
+	}
+
+	// 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/RelaxElevator.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc.team4737.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class RelaxElevator extends StopElevator {
+
+    public RelaxElevator() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    	super();
+    }
+
+    // Called just before this Command runs the first time
+//    protected void initialize() {
+//    }
+//
+//    // 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/StopElevator.java

@@ -1,7 +1,6 @@
 package org.usfirst.frc.team4737.robot.commands;
 
 import org.usfirst.frc.team4737.robot.Robot;
-import org.usfirst.frc.team4737.robot.subsystems.Elevator;
 
 import edu.wpi.first.wpilibj.command.Command;