Finished final tweaks for lift, also includes tests

Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java

@@ -7,6 +7,11 @@
 
 package org.usfirst.frc.team199.Robot2018;
 
+import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
+import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
+import org.usfirst.frc.team199.Robot2018.commands.LowerIntake;
+import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
+import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
 import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
@@ -35,21 +40,21 @@ public class OI {
 	private JoystickButton shiftLowGearButton;
 	private JoystickButton shiftHighGearButton;
 	private JoystickButton shiftDriveTypeButton;
-	private JoystickButton PIDMoveButton;
-	private JoystickButton PIDTurnButton;
+	private JoystickButton pIDMoveButton;
+	private JoystickButton pIDTurnButton;
 	private JoystickButton resetEncButton;
-	private JoystickButton MoveLiftUpButton;
-	private JoystickButton MoveLiftDownButton;
+	private JoystickButton moveLiftUpButton;
+	private JoystickButton moveLiftDownButton;
 	public Joystick rightJoy;
 	private JoystickButton updatePIDConstantsButton;
 	private JoystickButton updateEncoderDPPButton;
-	public Joystick manipulator;
-	private JoystickButton closeIntake;
-	private JoystickButton openIntake;
-	private JoystickButton raiseIntake;
-	private JoystickButton lowerIntake;
-	private JoystickButton intake;
-	private JoystickButton outake;
+	public static Joystick manipulator;
+	private JoystickButton closeIntakeButton;
+	private JoystickButton openIntakeButton;
+	private JoystickButton raiseIntakeButton;
+	private JoystickButton lowerIntakeButton;
+	private JoystickButton intakeCubeButton;
+	private JoystickButton outtakeCubeButton;
 
 	public int getButton(String key, int def) {
 		if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -65,13 +70,13 @@ public OI() {
 		leftJoy = new Joystick(0);
 		shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
 		shiftDriveTypeButton.whenPressed(new ShiftDriveType());
-		PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
-		PIDMoveButton
+		pIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
+		pIDMoveButton
 				.whenPressed(new PIDMove(Robot.sd.getConst("Move Targ", 24), Robot.dt, Robot.sd, RobotMap.distEncAvg));
-		PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
+		pIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
 		// PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90),
 		// Robot.dt, Robot.sd RobotMap.fancyGyro));
-		PIDTurnButton
+		pIDTurnButton
 				.whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro));
 		resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
 		resetEncButton.whenPressed(new ResetEncoders());
@@ -85,27 +90,24 @@ public OI() {
 		updatePIDConstantsButton.whenPressed(new UpdatePIDConstants());
 		updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
 		updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
-		MoveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
-		MoveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
-		MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
-		MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
 
 		// manipulator = new Joystick(2);
-		// closeIntake = new JoystickButton(manipulator, getButton("Close Intake
-		// Button", 1));
-		// closeIntake.whenPressed(new CloseIntake());
-		// openIntake = new JoystickButton(manipulator, getButton("Open Intake Button",
-		// 2));
-		// openIntake.whenPressed(new OpenIntake());
-		// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
-		// Button", 3));
-		// raiseIntake.whenPressed(new RaiseIntake());
-		// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
-		// Button", 4));
-		// lowerIntake.whenPressed(new LowerIntake());
-		// intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
-		// intake.whenPressed(new IntakeCube());
-		// outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
-		// outake.whenPressed(new OutakeCube());
+		//	closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
+		//	closeIntakeButton.whenPressed(new CloseIntake());
+		//	openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
+		//	openIntakeButton.whenPressed(new OpenIntake());
+		//	raiseIntakeButton = new JoystickButton(manipulator, getButton("Raise Intake Button", 3));
+		//	raiseIntakeButton.whenPressed(new RaiseIntake());
+		//	lowerIntakeButton = new JoystickButton(manipulator, getButton("Lower Intake Button", 4));
+		//	lowerIntakeButton.whenPressed(new LowerIntake());
+		//	intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
+		//	intakeCubeButton.whenPressed(new IntakeCube());
+		//	outtakeCubeButton = new JoystickButton(manipulator, getButton("Outtake Button", 6));
+		//	outtakeCubeButton.whenPressed(new OuttakeCube());
+
+		/*MoveLiftUpButton = new JoystickButton(manipulator, getButton("Run Lift Motor Up", 7));
+		MoveLiftDownButton = new JoystickButton(manipulator, getButton("Run Lift Motor Down", 8));
+		MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
+		MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));*/
 	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

@@ -36,12 +36,15 @@ public class RobotMap {
 
 	public static PowerDistributionPanel pdp;
 
-	public static WPI_TalonSRX liftMotor;
+	public static WPI_VictorSPX liftMotorA;
+	public static WPI_TalonSRX liftMotorB;
+	public static WPI_TalonSRX liftMotorC;
+	public static SpeedControllerGroup liftMotors;
 	public static Encoder liftEnc;
 	public static DigitalSource liftEncPort1;
 	public static DigitalSource liftEncPort2;
 
-	public static WPI_TalonSRX climberMotor;
+	//public static WPI_TalonSRX climberMotor;
 
 	public static VictorSP leftIntakeMotor;
 	public static VictorSP rightIntakeMotor;
@@ -118,15 +121,18 @@ private void configSPX(WPI_VictorSPX mc) {
 
 	public RobotMap() {
 		pdp = new PowerDistributionPanel();
-
-		liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 7));
-		configSRX(liftMotor);
+
+		liftMotorA = new WPI_VictorSPX(getPort("LiftVictorSPX", 5));
+		configSPX(liftMotorA);
+		liftMotorB = new WPI_TalonSRX(getPort("1LiftTalonSRX", 6));
+		configSRX(liftMotorB);
+		liftMotorC = new WPI_TalonSRX(getPort("2LiftTalonSRX", 7));
+		configSRX(liftMotorC);
+		liftMotors = new SpeedControllerGroup(liftMotorB, liftMotorA, liftMotorC);
 		liftEncPort1 = new DigitalInput(getPort("1LiftEnc", 4));
 		liftEncPort2 = new DigitalInput(getPort("2LiftEnc", 5));
 		liftEnc = new Encoder(liftEncPort1, liftEncPort2);
 		liftEnc.setPIDSourceType(PIDSourceType.kDisplacement);
-		climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6));
-		configSRX(climberMotor);
 
 		leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 9));
 		rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 8));

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java

@@ -14,55 +14,40 @@
  */
 public class AutoLift extends Command implements PIDOutput{
 	/**
-	 * All distances are - 1 foot for initial height of intake and + 3 inches for wiggle room for dropping cubes
-	 * Also, acutal distances are divided by 3 because according to cad, the lift will have a 1:3 ratio from winch
+	 * All distances are measured from bottom of cube and + 3 inches for wiggle room for dropping cubes
+	 * Also, actual distances are divided by 3 because according to cad, the lift will have a 1:3 ratio from winch
 	 * to actual height.
 	 */
 
 	/**
 	 * Distance to switch
 	 * 18.75 inches in starting position (this measurement is the fence that surrounds the switch)
-	 * 9.75 / 3 for ratio = 3.25
+	 * 21.75 / 3 for ratio = 7.25
 	 */
-	private final double SWITCH_DIST = 3.25;
+	private final double SWITCH_DIST = 7.25;
 	/**
 	 * Distance to scale
 	 * 5 feet starting
-	 * 51 / 3 = 17
+	 * 63 / 3 = 21
 	 */
-	private final double SCALE_DIST = 17;
+	private final double SCALE_DIST = 21;
 	/**
 	 * Distance to bar
-	 * 72 / 3 = 24
+	 * 87 / 3 = 29
 	 * 7 feet starting; bar distance should be changed because I'm not aware how climber mech will be positioned
 	 */
-	private final double BAR_DIST = 24;
+	private final double BAR_DIST = 29;
 	private double desiredDist = 0;
-	private double currDist = 0;
+	private double currDist;
 	private LiftInterface lift;
 	private Position desiredPos;
 
 	private PIDController liftController;
 
     public AutoLift(Position stage, LiftInterface lift) {
-        // Use requires() here to declare subsystem dependencies
-        // eg. requires(chassis);
     	this.lift = lift;
     	requires(Robot.lift);
-    	switch(lift.getCurrPos()) {
-    	case GROUND:
-    		currDist = 0;
-    		break;
-    	case SWITCH:
-    		currDist = SWITCH_DIST;
-    		break;
-    	case SCALE:
-    		currDist = SCALE_DIST;
-    		break;
-    	case BAR:
-    		currDist = BAR_DIST;
-    		break;
-    	}
+    	currDist = lift.getHeight();
     	switch(stage) {
     	case GROUND:
     		desiredDist = -currDist;
@@ -86,7 +71,6 @@ public AutoLift(Position stage, LiftInterface lift) {
 
     // Called just before this Command runs the first time
     protected void initialize() {
-    	lift.resetEnc();
 		// input is in inches
 		//liftController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204));
 		// output in "motor units" (arcade and tank only accept values [-1, 1]
@@ -104,7 +88,7 @@ protected void execute() {
     // Make this return true when this Command no longer needs to run execute()
     protected boolean isFinished() {
     	if(liftController.onTarget()) {
-    		lift.setTargetPosition(desiredPos);
+    		lift.setCurrPosition(desiredPos);
     		return true;
     	}
     	return false;

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java → Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OuttakeCube.java

@@ -8,11 +8,11 @@
 /**
  *
  */
-public class OutakeCube extends Command {
+public class OuttakeCube extends Command {
 
 	Timer tim;
 
-	public OutakeCube() {
+	public OuttakeCube() {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.wpilibj.command.InstantCommand;
 
 /**
- *
+ * Resets left and right drivetrain encoders to 0
  */
 public class ResetEncoders extends InstantCommand {
 

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/SetDistancePerPulse.java

@@ -5,7 +5,8 @@
 import edu.wpi.first.wpilibj.command.InstantCommand;
 
 /**
- *
+ * Sets distance per pulse for left and right drivetrain encoders from
+ * SmartDashboard "Distance Per Pulse Left" & "Distance Per Pulse Right"
  */
 public class SetDistancePerPulse extends InstantCommand {
 

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdatePIDConstants.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.wpilibj.command.InstantCommand;
 
 /**
- *
+ * Updates the PIDControllers' PIDConstants based on SmartDashboard values
  */
 public class UpdatePIDConstants extends InstantCommand {
 

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Climber.java

@@ -11,7 +11,7 @@
  */
 public class Climber extends Subsystem implements ClimberInterface {
 
-	private final WPI_TalonSRX climberMotor = RobotMap.climberMotor;
+	//private final WPI_TalonSRX climberMotor = RobotMap.climberMotor;
 
 
 	/**
@@ -52,7 +52,7 @@ public void attachToBar() {
 	 * stops the climber
 	 */
 	public void stopClimber() {
-		climberMotor.stopMotor();
+		//climberMotor.stopMotor();
 	}