Merge branch 'lift-merging' of https://github.com/Gabe-Mitnick/RobotCode2018.git into lift

Author: lhmcgann

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

@@ -12,103 +12,105 @@
 /**
  *
  */
-public class AutoLift extends Command implements PIDOutput{
+public class AutoLift extends Command implements PIDOutput {
 	/**
-	 * 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.
+	 * 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)
-	 * 21.75 / 3 for ratio = 7.25
-	 */
-	private final double SWITCH_DIST = 7.25;
-	/**
-	 * Distance to scale
-	 * 5 feet starting
-	 * 63 / 3 = 21
-	 */
-	private final double SCALE_DIST = 21;
-	/**
-	 * Distance to bar
-	 * 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 = 29;
+
+	private final int NUM_STAGES;
+	private final double WIGGLE_ROOM;
+	private final double SWITCH_DIST;
+	private final double SCALE_DIST;
+	private final double BAR_DIST;
+
 	private double desiredDist = 0;
 	private double currDist;
 	private LiftInterface lift;
 	private Position desiredPos;
-	
+
 	private PIDController liftController;
 
-    public AutoLift(Position stage, LiftInterface lift) {
-    	this.lift = lift;
-    	requires(Robot.lift);
-    	currDist = lift.getHeight();
-    	switch(stage) {
-    	case GROUND:
-    		desiredDist = -currDist;
-    		break;
-    	case SWITCH:
-    		desiredDist = SWITCH_DIST - currDist;
-    		break;
-    	case SCALE:
-    		desiredDist = SCALE_DIST - currDist;
-    		break;
-    	case BAR:
-    		desiredDist = BAR_DIST - currDist;
-    		break;
-    	}
-    	
-    	liftController = new PIDController(Robot.getConst("LiftkP", 1), Robot.getConst("LiftkI", 0),
-				Robot.getConst("LiftkD", 0),RobotMap.liftEnc, this);
-    	
-    	desiredPos = stage;
-    }
-
-    // Called just before this Command runs the first time
-    protected void initialize() {
+	public AutoLift(Position stage, LiftInterface lift) {
+		this.lift = lift;
+		requires(Robot.lift);
+
+		currDist = lift.getHeight();
+
+		// calculate constant measurements
+
+		NUM_STAGES = lift.getNumStages();
+		WIGGLE_ROOM = lift.getWiggleRoom();
+		// 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
+		SCALE_DIST = (60.0 + WIGGLE_ROOM) / NUM_STAGES;
+		// 7 feet starting; bar distance should be changed because I'm not aware how
+		// climber mech will be positioned
+		BAR_DIST = (84.0 + WIGGLE_ROOM) / NUM_STAGES;
+
+		switch (stage) {
+		case GROUND:
+			desiredDist = -currDist;
+			break;
+		case SWITCH:
+			desiredDist = SWITCH_DIST - currDist;
+			break;
+		case SCALE:
+			desiredDist = SCALE_DIST - currDist;
+			break;
+		case BAR:
+			desiredDist = BAR_DIST - currDist;
+			break;
+		}
+
+		liftController = new PIDController(Robot.getConst("LiftkP", 1), Robot.getConst("LiftkI", 0),
+				Robot.getConst("LiftkD", 0), RobotMap.liftEnc, this);
+
+		desiredPos = stage;
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
 		// input is in inches
-		//liftController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204));
+		// 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]
 		liftController.setOutputRange(-1.0, 1.0);
 		liftController.setContinuous(false);
-		//liftController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
+		// liftController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
 		liftController.setSetpoint(desiredDist);
 		liftController.enable();
-    }
-
-    // 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() {
-    	if(liftController.onTarget()) {
-    		lift.setCurrPosition(desiredPos);
-    		return true;
-    	}
-    	return false;
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-    	liftController.disable();
-    	liftController.free();
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    protected void interrupted() {
-    	end();
-    }
-    
-    @Override
+	}
+
+	// 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() {
+		if (liftController.onTarget()) {
+			lift.setCurrPosition(desiredPos);
+			return true;
+		}
+		return false;
+	}
+
+	// Called once after isFinished returns true
+	protected void end() {
+		liftController.disable();
+		liftController.free();
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	protected void interrupted() {
+		end();
+	}
+
+	@Override
 	public void pidWrite(double output) {
 		lift.runMotor(output);
 	}
-    
 }

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

@@ -18,6 +18,14 @@ public class Lift extends Subsystem implements LiftInterface {
 	private final Encoder liftEnc = RobotMap.liftEnc;
 	private Position currPosition = Position.GROUND;
 
+	private final int NUM_STAGES;
+	private final double WIGGLE_ROOM;
+
+	public Lift() {
+		NUM_STAGES = (int) Robot.getConst("Lift stages", 1);
+		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0);
+	}
+
 	/**
 	 * Set the default command for a subsystem here.
 	 */
@@ -51,7 +59,7 @@ public void setCurrPosition(Position newPosition) {
 	 * Uses (insert sensor here) to detect the current lift position
 	 */
 	public double getHeight() {
-		return liftEnc.getDistance() * 3;
+		return liftEnc.getDistance() * NUM_STAGES;
 	}
 
 	/**
@@ -94,4 +102,17 @@ public void resetEnc() {
 		liftEnc.reset();
 	}
 
+	/**
+	 * Gets the number of stages variable
+	 */
+	public int getNumStages() {
+		return NUM_STAGES;
+	}
+
+	/**
+	 * Gets the extra distance above the switch or scale we want to lift in inches
+	 */
+	public double getWiggleRoom() {
+		return WIGGLE_ROOM;
+	}
 }

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

@@ -1,58 +1,66 @@
 package org.usfirst.frc.team199.Robot2018.subsystems;
 
-import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
-
 public interface LiftInterface {
-	
+
 	/**
 	 * Set the default command for a subsystem here.
-	 * */
+	 */
 	public void initDefaultCommand();
-	
+
 	public enum Position {
-		GROUND,
-		SWITCH,
-		SCALE,
-		BAR
+		GROUND, SWITCH, SCALE, BAR
 	}
-	
+
 	/**
-	 * Uses (insert sensor here) to detect the current lift position 
+	 * Uses (insert sensor here) to detect the current lift position
 	 */
 	public double getHeight();
-	
+
 	/**
 	 * stops the lift
 	 */
 	public void stopLift();
-	
+
 	/**
 	 * gets current motor values
 	 */
 	public double getLiftSpeed();
-	
+
 	/**
 	 * Runs lift motors at specified speed
-	 * @param speed - desired speed to run at
+	 * 
+	 * @param speed
+	 *            - desired speed to run at
 	 */
 	public void runMotor(double speed);
-	
+
 	/**
 	 * Returns the position the lift is currently at
+	 * 
 	 * @return pos - current position
 	 */
 	public Position getCurrPos();
-	
+
 	/**
 	 * Resets the encoder
 	 */
 	public void resetEnc();
-	
+
 	/**
 	 * Sets the current position in the lift subsystem
-	 * @param newPosition - the new position meant to be set
+	 * 
+	 * @param newPosition
+	 *            - the new position meant to be set
 	 */
 	public void setCurrPosition(Position newPosition);
-	
-	
+
+	/**
+	 * Gets the number of stages variable
+	 */
+	public int getNumStages();
+
+	/**
+	 * Gets the extra distance above the switch or scale we want to lift in inches
+	 */
+	public double getWiggleRoom();
 }