Mapped 4 stages of lift to manipulator POV for tele, also minor tweaks

The 4 stages of lift are now mapped to POV for teleop. Also did tweaks
like motor ports, slaves, etc. Also note that manipulator in OI is
static now for default command in lift.

Author: nelly-yy

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

@@ -43,7 +43,7 @@ public class OI {
 	public Joystick rightJoy;
 	private JoystickButton updatePIDConstantsButton;
 	private JoystickButton updateEncoderDPPButton;
-	public Joystick manipulator;
+	public static Joystick manipulator;
 	private JoystickButton closeIntake;
 	private JoystickButton openIntake;
 	private JoystickButton raiseIntake;
@@ -85,10 +85,6 @@ 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
@@ -107,5 +103,10 @@ public OI() {
 		// intake.whenPressed(new IntakeCube());
 		// outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
 		// outake.whenPressed(new OutakeCube());
+		
+		/*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/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();
 	}
 
 

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

@@ -1,77 +1,89 @@
 package org.usfirst.frc.team199.Robot2018.subsystems;
 
+import org.usfirst.frc.team199.Robot2018.OI;
 import org.usfirst.frc.team199.Robot2018.RobotMap;
+import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
 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.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 /**
  *
  */
 public class Lift extends Subsystem implements LiftInterface {
 
-	private final WPI_TalonSRX liftMotor = RobotMap.liftMotor;
+	private final SpeedControllerGroup liftMotors = RobotMap.liftMotors;
 	private final Encoder liftEnc = RobotMap.liftEnc;
-	private Position targetPosition = Position.GROUND;
+	private Position currPosition = Position.GROUND;
 
 	/**
 	 * Set the default command for a subsystem here.
 	 * */
     public void initDefaultCommand() {
         // Set the default command for a subsystem here.
         //setDefaultCommand(new MySpecialCommand());
+    	int ang = OI.manipulator.getPOV();
+    	Command com = null;
+    	if(ang == 0)
+    		com = new AutoLift(Position.GROUND, this);
+    	else if(ang == 90)
+    		com = new AutoLift(Position.SWITCH, this);
+    	else if(ang == 180)
+    		com = new AutoLift(Position.SWITCH, this);
+    	else if(ang == 270)
+    		com = new AutoLift(Position.SWITCH, this);
+    	if(com != null)
+    		setDefaultCommand(com);
     }
 
-    public void setTargetPosition(Position newPosition) {
-    		targetPosition = newPosition;
+    /**
+	 * Sets the current position in the lift subsystem
+	 * @param newPosition - the new position meant to be set
+	 */
+    public void setCurrPosition(Position newPosition) {
+    		currPosition = newPosition;
     }
 
     /**
 	 * Uses (insert sensor here) to detect the current lift position 
 	 */
 	public double getHeight() {
-		return -1;
+		return liftEnc.getDistance() * 3;
 	}
 
 	/**
 	 * stops the lift
 	 */
 	public void stopLift() {
-		liftMotor.stopMotor();
+		liftMotors.stopMotor();
 	}
 
 	/**
 	 * gets current motor values
 	 */
 	public double getLiftSpeed() {
-		return liftMotor.get();
+		return liftMotors.get();
 	}
 
 	/**
-	 * Goes to specified height
-	 * @param position - ground, switch, scale, bar
-	 * @param offset - distance up or down from the position
-	 */
-	public void goToPosition(Position position, double offset) {
-		
-	}
-	/**
-	 * Runs lift motor at specified speed
+	 * Runs lift motors at specified speed
 	 * @param speed - desired speed to run at
 	 */
 	public void runMotor(double output) {
-		liftMotor.set(output);
+		liftMotors.set(output);
 	}
 
 	/**
 	 * Returns the position the lift is currently at
 	 * @return pos - current position
 	 */
 	public Position getCurrPos() {
-		return targetPosition;
+		return currPosition;
 	}
 	/**
 	 * Resets the encoder

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

@@ -31,16 +31,8 @@ public enum Position {
 	 */
 	public double getLiftSpeed();
 
-	
-	/**
-	 * Goes to specified height
-	 * @param position - ground, switch, scale, bar
-	 * @param offset - distance up or down from position
-	 */
-	public void goToPosition(Position position, double offset);
-	
 	/**
-	 * Runs lift motor at specified speed
+	 * Runs lift motors at specified speed
 	 * @param speed - desired speed to run at
 	 */
 	public void runMotor(double speed);
@@ -60,5 +52,7 @@ public enum Position {
 	 * Sets the current position in the lift subsystem
 	 * @param newPosition - the new position meant to be set
 	 */
-	public void setTargetPosition(Position newPosition);
+	public void setCurrPosition(Position newPosition);
+	
+	
 }