implemented HOLD_CUBE as first targ when told to go to the GROUND

for UpdateLiftPosition and LiftToPosition

Author: lhmcgann

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

@@ -181,6 +181,8 @@ public void robotInit() {
 		listen = new Listener();
 		// CameraServer.getInstance().startAutomaticCapture(0);
 		// CameraServer.getInstance().startAutomaticCapture(1);
+
+		lift.resetEnc();
 	}
 
 	/**
@@ -207,7 +209,6 @@ public void disabledPeriodic() {
 	@Override
 	public void autonomousInit() {
 		dt.resetAHRS();
-		lift.resetEnc();
 		AutoUtils.state = new State(0, 0, 0);
 		Scheduler.getInstance().add(new ShiftLowGear());
 		Scheduler.getInstance().add(new CloseIntake());

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

@@ -13,6 +13,7 @@ public class LiftToPosition extends Command {
 
 	private Lift lift;
 	private LiftHeight pos;
+	private boolean goToGround;
 
 	public LiftToPosition(Lift lift, String goal) {
 		requires(Robot.lift);
@@ -25,14 +26,14 @@ public LiftToPosition(Lift lift, String goal) {
 	protected void initialize() {
 		lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0),
 				Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1));
-		double setpoint = lift.getDesiredDistFromPos(pos);
-		lift.setSetpoint(setpoint);
-		System.out.println("Target Height: " + setpoint);
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
+		double setpoint = lift.getDesiredDistFromPos(pos);
+		lift.setSetpoint(setpoint);
+		System.out.println("Target Height: " + setpoint);
 		System.out.println("Current Height: " + lift.getHeight());
 	}
 

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

@@ -16,7 +16,7 @@ public class UpdateLiftPosition extends Command {
 	private LiftHeight desiredPos;
 
 	private boolean manipulatorPluggedIn = true;
-	private boolean goToGround = false;
+	private boolean goToGround;
 
 	public UpdateLiftPosition(Lift lift) {
 		requires(Robot.lift);
@@ -32,6 +32,7 @@ protected void initialize() {
 			System.err.println("[ERROR] Manipulator not plugged in.");
 			manipulatorPluggedIn = false;
 		}
+		goToGround = false;
 	}
 
 	// Called repeatedly when this Command is scheduled to run
@@ -42,18 +43,8 @@ protected void execute() {
 
 			System.out.println("POV Reading: " + angle);
 
-			// 180 degrees is down and -1 is "nothing," so basically default to GROUND,
-			// anything else is SWITCH
-			// if (angle != 180 && angle != -1) {
-			// desiredPos = LiftHeight.SWITCH;
-			// // NOTE: if full lift functionality does become a thing, need to add a couple
-			// // more if-elses here to account for those enum values
-			// } else {
-			// desiredPos = LiftHeight.GROUND;
-			// }
-
 			if (angle == 180) {
-				desiredPos = LiftHeight.HOLD_CUBE;
+				desiredPos = LiftHeight.GROUND;
 				goToGround = true;
 			} else if (angle == 270) {
 				desiredPos = LiftHeight.HOLD_CUBE;
@@ -63,17 +54,17 @@ protected void execute() {
 				goToGround = false;
 			}
 
-			if (angle != -1) {
+			if (goToGround || angle != -1) {
 				desiredDist = lift.getDesiredDistFromPos(desiredPos);
 				lift.setSetpoint(desiredDist);
 			}
 
-			if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
-				desiredPos = LiftHeight.GROUND;
-				desiredDist = lift.getDesiredDistFromPos(desiredPos);
-				lift.setSetpoint(desiredDist);
-				goToGround = false;
-			}
+			// if (goToGround && lift.onTarget() && lift.getSpeed() <= 0.1) {
+			// desiredPos = LiftHeight.GROUND;
+			// desiredDist = lift.getDesiredDistFromPos(desiredPos);
+			// lift.setSetpoint(desiredDist);
+			// goToGround = false;
+			// }
 		}
 	}
 

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

@@ -20,10 +20,15 @@ public class Lift extends PIDSubsystem implements LiftInterface {
 
 	private final int NUM_STAGES;
 	private final double WIGGLE_ROOM;
+	private final double GROUND_DIST;
+	private final double HOLD_CUBE_DIST;
 	private final double SWITCH_DIST;
 	private final double SCALE_DIST;
 	private final double BAR_DIST;
 
+	private LiftHeight desiredPos;
+	private boolean goToGround;
+
 	public Lift() {
 		super("Lift", 0, 0, 0, 0);
 
@@ -35,7 +40,7 @@ public Lift() {
 		double kP = r / getLiftTimeConstant() / maxSpeed;
 		double kI = 0;
 		double kD = r / maxSpeed;
-		double kF = 1 / (maxSpeed * Robot.getConst("Default PID Update Time", 0.05));
+		double kF = 1 / maxSpeed * Robot.getConst("Default PID Update Time", 0.05);
 
 		PIDController liftController = getPIDController();
 
@@ -56,6 +61,8 @@ public Lift() {
 		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0); // inches
 
 		// calculate constant measurements
+		GROUND_DIST = 0;
+		HOLD_CUBE_DIST = 4;
 		// distance to switch 18.75 inches in starting position
 		SWITCH_DIST = (18.75 + WIGGLE_ROOM) / NUM_STAGES;
 		// distance to scale 5 feet starting 63 / 3 = 21
@@ -74,37 +81,91 @@ public void initDefaultCommand() {
 		setDefaultCommand(new UpdateLiftPosition(this));
 	}
 
+	/**
+	 * @return whether or not the cube
+	 */
+	private boolean atHoldCube() {
+		double tol = Robot.getConst("Lift Tolerance", 0.8);
+		double height = getHeight();
+		// true if (height within tolerance of HOLD_CUBE height) and (lift is not
+		// moving)
+		return height <= (HOLD_CUBE_DIST + tol) && height >= (HOLD_CUBE_DIST - tol) && getSpeed() <= 0.1;
+	}
+
+	/**
+	 * @return true if reached target height and does not still have to go to the
+	 *         ground as a secondary, consecutive target
+	 */
+	@Override
+	public boolean onTarget() {
+		double targ = getSetpoint();
+		double tol = Robot.getConst("Lift Tolerance", 0.8);
+		double height = getHeight();
+		// true if (height within tolerance of targ) and (lift is not moving)
+		boolean wInTol = height <= (targ + tol) && height >= (targ - tol) && getSpeed() <= 0.1;
+		return wInTol && !goToGround;
+	}
+
+	/**
+	 * Set the lift's setpoint to this value continuously.
+	 * 
+	 * @param pos
+	 *            - the desired FINAL height
+	 * @return the FIRST height to travel to; in all cases but GROUND, FIRST = FINAL
+	 *         targ
+	 */
 	public double getDesiredDistFromPos(LiftHeight pos) {
 		double desiredDist;
-		switch (pos) {
+		LiftHeight height = pos;
+		switch (height) {
 		case GROUND:
-			desiredDist = 0;
-			break;
+			if (atHoldCube()) {
+				desiredDist = GROUND_DIST;
+				// false bc doesn't need to hold the GROUND as a second consecutive targ anymore
+				goToGround = false;
+				break;
+			} else {
+				// falls through to HOLD_CUBE case
+				height = LiftHeight.HOLD_CUBE;
+				goToGround = true;
+			}
 		case HOLD_CUBE:
-			desiredDist = 4;
+			desiredDist = HOLD_CUBE_DIST;
+			goToGround = false;
 			break;
 		case SWITCH:
 			desiredDist = SWITCH_DIST;
+			goToGround = false;
 			break;
 		case SCALE:
 			desiredDist = SCALE_DIST;
+			goToGround = false;
 			break;
 		case BAR:
 			desiredDist = BAR_DIST;
+			goToGround = false;
 			break;
 		default:
-			desiredDist = 0;
+			desiredDist = GROUND_DIST;
+			goToGround = false;
 			break;
 		}
 
 		return desiredDist;
 	}
 
 	/**
-	 * @return the rate of the lift encoder (in/s)
+	 * @return the desired LiftHeight to be at
+	 */
+	public LiftHeight desiredPos() {
+		return desiredPos;
+	}
+
+	/**
+	 * @return the absolute rate of the lift encoder (in/s)
 	 */
 	public double getSpeed() {
-		return liftEnc.getRate();
+		return Math.abs(liftEnc.getRate());
 	}
 
 	/**