merged nelson's code

Robot2018/lib/User_Libraries.properties

@@ -1,2 +1,2 @@
-#Sat Feb 17 17:04:37 PST 2018
+#Sun Feb 18 11:01:27 PST 2018
 C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4

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

@@ -16,6 +16,7 @@
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
 import org.usfirst.frc.team199.Robot2018.commands.RaiseIntake;
 import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
+import org.usfirst.frc.team199.Robot2018.commands.RunLift;
 import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
@@ -39,6 +40,8 @@ public class OI {
 	private JoystickButton PIDMoveButton;
 	private JoystickButton PIDTurnButton;
 	private JoystickButton resetEncButton;
+	private JoystickButton MoveLiftUpButton;
+	private JoystickButton MoveLiftDownButton;
 	public Joystick rightJoy;
 	private JoystickButton updatePIDConstantsButton;
 	private JoystickButton updateEncoderDPPButton;
@@ -84,6 +87,10 @@ 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));

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

@@ -37,6 +37,10 @@ public class RobotMap {
 	public static PowerDistributionPanel pdp;
 
 	public static WPI_TalonSRX liftMotor;
+	public static Encoder liftEnc;
+	public static DigitalSource liftEncPort1;
+	public static DigitalSource liftEncPort2;
+
 	public static WPI_TalonSRX climberMotor;
 
 	public static VictorSP leftIntakeMotor;
@@ -115,12 +119,14 @@ private void configSPX(WPI_VictorSPX mc) {
 	public RobotMap() {
 		pdp = new PowerDistributionPanel();
 
-		// intakeMotor = new WPI_TalonSRX(getPort("IntakeTalonSRX", 4));
-		// configSRX(intakeMotor);
-		// liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 5));
-		// configSRX(liftMotor);
-		// climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6));
-		// configSRX(climberMotor);
+		liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 7));
+		configSRX(liftMotor);
+		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

@@ -0,0 +1,130 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.RobotMap;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+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
+	 * 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
+	 */
+	private final double SWITCH_DIST = 3.25;
+	/**
+	 * Distance to scale
+	 * 5 feet starting
+	 * 51 / 3 = 17
+	 */
+	private final double SCALE_DIST = 17;
+	/**
+	 * Distance to bar
+	 * 72 / 3 = 24
+	 * 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 double desiredDist = 0;
+	private double currDist = 0;
+	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;
+    	}
+    	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() {
+    	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]
+		liftController.setOutputRange(-1.0, 1.0);
+		liftController.setContinuous(false);
+		//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.setTargetPosition(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/commands/RunLift.java

@@ -0,0 +1,52 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class RunLift extends Command {
+
+	private LiftInterface lift;
+	private final double SPEED = 0.05;
+	private int dir;
+
+	public RunLift(LiftInterface lift, boolean up) {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+		this.lift = lift;
+		requires(Robot.lift);
+		if (up)
+			dir = 1;
+		else
+			dir = -1;
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		lift.runMotor(SPEED * dir);
+	}
+
+	// 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() {
+		lift.stopLift();
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	protected void interrupted() {
+		end();
+	}
+}

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

@@ -1,9 +1,11 @@
 package org.usfirst.frc.team199.Robot2018.subsystems;
 
 import org.usfirst.frc.team199.Robot2018.RobotMap;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 /**
@@ -12,7 +14,7 @@
 public class Lift extends Subsystem implements LiftInterface {
 
 	private final WPI_TalonSRX liftMotor = RobotMap.liftMotor;
-
+	private final Encoder liftEnc = RobotMap.liftEnc;
 	private Position targetPosition = Position.GROUND;
 
 	/**
@@ -56,7 +58,27 @@ public double getLiftSpeed() {
 	public void goToPosition(Position position, double offset) {
 
 	}
+	/**
+	 * Runs lift motor at specified speed
+	 * @param speed - desired speed to run at
+	 */
+	public void runMotor(double output) {
+		liftMotor.set(output);
+	}
 
+	/**
+	 * Returns the position the lift is currently at
+	 * @return pos - current position
+	 */
+	public Position getCurrPos() {
+		return targetPosition;
+	}
+	/**
+	 * Resets the encoder
+	 */
+	public void resetEnc() {
+		liftEnc.reset();
+	}
 
 }
 

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

@@ -1,5 +1,7 @@
 package org.usfirst.frc.team199.Robot2018.subsystems;
 
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.Position;
+
 public interface LiftInterface {
 
 	/**
@@ -37,5 +39,26 @@ public enum Position {
 	 */
 	public void goToPosition(Position position, double offset);
 
+	/**
+	 * Runs lift motor at specified speed
+	 * @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
+	 */
+	public void setTargetPosition(Position newPosition);
 }