new lift setup

made Lift extend PIDSubsystem and implemented the necessary methods for
that (need to do spd --> voltage method though)
made new command (UpdateLiftPosition) for continuous teleop PID -->
default Lift command
need to make commands for auto

Author: lhmcgann

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

@@ -59,6 +59,7 @@ public class Robot extends IterativeRobot {
 	String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
 
 	public static SmartDashboardInterface sd = new SmartDashboardInterface() {
+		@Override
 		public double getConst(String key, double def) {
 			Preferences pref = Preferences.getInstance();
 			if (!pref.containsKey("Const/" + key)) {
@@ -71,6 +72,7 @@ public double getConst(String key, double def) {
 			return pref.getDouble("Const/" + key, def);
 		}
 
+		@Override
 		public void putConst(String key, double def) {
 			Preferences pref = Preferences.getInstance();
 			pref.putDouble("Const/" + key, def);
@@ -79,14 +81,17 @@ public void putConst(String key, double def) {
 			}
 		}
 
+		@Override
 		public void putData(String string, PIDController controller) {
 			SmartDashboard.putData(string, controller);
 		}
 
+		@Override
 		public void putNumber(String string, double d) {
 			SmartDashboard.putNumber(string, d);
 		}
 
+		@Override
 		public void putBoolean(String string, boolean b) {
 			SmartDashboard.putBoolean(string, b);
 		}
@@ -130,7 +135,8 @@ public void robotInit() {
 		climber = new Climber();
 		climberAssist = new ClimberAssist();
 		intakeEject = new IntakeEject();
-		lift = new Lift();
+		lift = new Lift("Lift", getConst("LiftkP", 0.1), getConst("LiftkI", 0), getConst("LiftkD", 0),
+				getConst("LiftkF", 0.1));
 		dt = new Drivetrain(sd);
 		oi = new OI(this);
 

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

@@ -0,0 +1,64 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This command should be the default command for the Lift subsystem
+ */
+public class UpdateLiftPosition extends Command {
+
+	private Lift lift;
+	private double targ;
+
+	public UpdateLiftPosition(Lift lift) {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+		this.lift = lift;
+		targ = 0;
+	}
+
+	// Called just before this Command runs the first time
+	@Override
+	protected void initialize() {
+		/**
+		 * May need to setInputRange(0, maxHeight);
+		 */
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	@Override
+	protected void execute() {
+		int angle = Robot.oi.manipulator.getPOV();
+		/**
+		 * reference AutoLift for how to calculate targ (below) based on the given
+		 * Position enum
+		 */
+		if (angle == 0) {
+			// targ = GROUND
+		} else {
+			// targ = SWITCH
+		}
+		lift.setSetpoint(targ);
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	@Override
+	protected boolean isFinished() {
+		return false;
+	}
+
+	// Called once after isFinished returns true
+	@Override
+	protected void end() {
+		lift.disable();
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	@Override
+	protected void interrupted() {
+	}
+}

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

@@ -2,17 +2,16 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.RobotMap;
-import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
+import org.usfirst.frc.team199.Robot2018.commands.UpdateLiftPosition;
 
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
 /**
  *
  */
-public class Lift extends Subsystem implements LiftInterface {
+public class Lift extends PIDSubsystem implements LiftInterface {
 
 	private final SpeedControllerGroup liftMotors = RobotMap.liftMotors;
 	private final Encoder liftEnc = RobotMap.liftEnc;
@@ -21,28 +20,19 @@ public class Lift extends Subsystem implements LiftInterface {
 	private final int NUM_STAGES;
 	private final double WIGGLE_ROOM;
 
-	public Lift() {
+	public Lift(String name, double kP, double kI, double kD, double kF) {
+		super(name, kP, kI, kD, kF);
 		NUM_STAGES = (int) Robot.getConst("Lift stages", 1);
 		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0);
+		enable();
 	}
 
 	/**
 	 * Set the default command for a subsystem here.
 	 */
+	@Override
 	public void initDefaultCommand() {
-		// Set the default command for a subsystem here.
-		// setDefaultCommand(new MySpecialCommand());
-		int angle = Robot.oi.manipulator.getPOV();
-
-		Command com = null;
-
-		if (angle == 0) {
-			com = new AutoLift(Position.GROUND, this);
-			setDefaultCommand(com);
-		} else if (angle == 90 || angle == 180 || angle == 270) {
-			com = new AutoLift(Position.SWITCH, this);
-			setDefaultCommand(com);
-		}
+		setDefaultCommand(new UpdateLiftPosition(this));
 	}
 
 	/**
@@ -51,27 +41,31 @@ public void initDefaultCommand() {
 	 * @param newPosition
 	 *            - the new position meant to be set
 	 */
+	@Override
 	public void setCurrPosition(Position newPosition) {
 		currPosition = newPosition;
 	}
 
 	/**
 	 * Uses (insert sensor here) to detect the current lift position
 	 */
+	@Override
 	public double getHeight() {
 		return liftEnc.getDistance() * NUM_STAGES;
 	}
 
 	/**
 	 * stops the lift
 	 */
+	@Override
 	public void stopLift() {
 		liftMotors.stopMotor();
 	}
 
 	/**
 	 * gets current motor values
 	 */
+	@Override
 	public double getLiftSpeed() {
 		return liftMotors.get();
 	}
@@ -82,6 +76,7 @@ public double getLiftSpeed() {
 	 * @param speed
 	 *            - desired speed to run at
 	 */
+	@Override
 	public void runMotor(double output) {
 		liftMotors.set(output);
 	}
@@ -91,28 +86,65 @@ public void runMotor(double output) {
 	 * 
 	 * @return pos - current position
 	 */
+	@Override
 	public Position getCurrPos() {
 		return currPosition;
 	}
 
 	/**
 	 * Resets the encoder
 	 */
+	@Override
 	public void resetEnc() {
 		liftEnc.reset();
 	}
 
 	/**
 	 * Gets the number of stages variable
 	 */
+	@Override
 	public int getNumStages() {
 		return NUM_STAGES;
 	}
 
 	/**
 	 * Gets the extra distance above the switch or scale we want to lift in inches
 	 */
+	@Override
 	public double getWiggleRoom() {
 		return WIGGLE_ROOM;
 	}
+
+	/**
+	 * Uses AMT103 Encoder to detect the current lift height with respect to the
+	 * lift's min height (inches)
+	 */
+	@Override
+	protected double returnPIDInput() {
+		// return getHeight(); //Use this instead? What's the difference?
+		return liftEnc.getDistance();
+	}
+
+	/**
+	 * Runs the lift motors at the value that the pid loop calculated. Used
+	 * internally by PIDSubsystem.
+	 */
+	@Override
+	protected void usePIDOutput(double output) {
+		double out = output;
+		double spd = liftEnc.getRate();
+		out += convertSpdToVoltage(spd);
+		runMotor(out);
+	}
+
+	/**
+	 * Takes a value for the current lift speed and translates it to the amount of
+	 * voltage to motors need to supply.
+	 * 
+	 * @param speed
+	 *            - the current lift speed (in/s)
+	 */
+	public double convertSpdToVoltage(double speed) {
+		return 0;
+	}
 }