Finish Gatorade shoulder (#26)

src/main/java/com/team766/robot/Robot.java

@@ -7,6 +7,7 @@ public class Robot {
 	public static Intake intake;
 	public static Wrist wrist;
 	public static Elevator elevator;
+	public static Shoulder shoulder;
 	public static Drive drive;
 	public static Gyro gyro;
 	public static Lights lights;
@@ -16,6 +17,7 @@ public static void robotInit() {
 		intake = new Intake();
 		wrist = new Wrist();
 		elevator = new Elevator();
+		shoulder = new Shoulder();
 		drive = new Drive();
 		gyro = new Gyro();
 		lights = new Lights();

src/main/java/com/team766/robot/mechanisms/Elevator.java

@@ -1,4 +1,6 @@
 package com.team766.robot.mechanisms;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkMaxPIDController;
 import com.revrobotics.CANSparkMax.ControlType;
@@ -25,15 +27,15 @@ public enum Position {
 		/** Elevator is fully retracted.  Starting position. */
 		RETRACTED(0),
 		/** Elevator is the appropriate height to place game pieces at the low node. */
-		LOW(5), 
+		LOW(0),
 		/** Elevator is the appropriate height to place game pieces at the mid node. */
 		MID(18),
 		/** Elevator is at appropriate height to place game pieces at the high node. */
 		HIGH(40), 
 		/** Elevator is at appropriate height to grab cubes from the human player. */
-		HUMAN_CUBES(40.5),
+		HUMAN_CUBES(39),
 		/** Elevator is at appropriate height to grab cones from the human player. */
-		HUMAN_CONES(40.5),
+		HUMAN_CONES(40),
 		/** Elevator is fully extended. */
 		EXTENDED(40);
 
@@ -48,7 +50,7 @@ private double getHeight() {
 		}
 	}
 
-	private static final double NUDGE_INCREMENT = 5.0;
+	private static final double NUDGE_INCREMENT = 2.0;
 	private static final double NUDGE_DAMPENER = 0.25;
 
 	private static final double NEAR_THRESHOLD = 2.0;
@@ -78,6 +80,9 @@ public Elevator() {
 			throw new IllegalStateException("Motor are not CANSparkMaxes!");
 		}
 
+		halLeftMotor.setNeutralMode(NeutralMode.Brake);
+		halRightMotor.setNeutralMode(NeutralMode.Brake);
+
 		leftMotor = (CANSparkMax) halLeftMotor;
 		rightMotor = (CANSparkMax) halRightMotor;
 

src/main/java/com/team766/robot/mechanisms/EncoderUtils.java

@@ -48,16 +48,16 @@ public static double elevatorRotationsToHeight(double rotations) {
 	 * Converts a target rotation (in degrees) to encoder units for the shoulder motor.
 	 */
 	public static double shoulderDegreesToRotations(double angle) {
-		// angle * net gear ratio * (rotations / degrees)
-		return angle * (4. / 1.) * (3. / 1.) * (1. / 360.);
+		// angle * sprocket ratio * net gear ratio * (rotations / degrees)
+		return angle * (52.0 / 12.0) * (64.0 / 30.0) * (4. / 1.) * (3. / 1.) * (1. / 360.);
 	}
 
 	/**
 	 * Converts the shoulder motor's rotations to degrees.
 	 */
 	public static double shoulderRotationsToDegrees(double rotations) {
-		// rotations * net gear ratio * (degrees / rotations)
-		return rotations * (1. / 4.) * (1. / 3.) * (360. / 1.);
+		// rotations * sprocket ratio * net gear ratio * (degrees / rotations)
+		return rotations * (12.0 / 52.0) * (30.0 / 64.0) * (1. / 4.) * (1. / 3.) * (360. / 1.);
 	}
 
 	/**

src/main/java/com/team766/robot/mechanisms/Shoulder.java

@@ -1,7 +1,9 @@
 package com.team766.robot.mechanisms;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMax.ExternalFollower;
 import com.revrobotics.SparkMaxPIDController;
 import com.team766.config.ConfigFileReader;
 import com.team766.framework.Mechanism;
@@ -28,22 +30,25 @@ public enum Position {
 
 		// TODO: adjust these!
 
-		/** Shoulder is in top position. */
+		/** Shoulder is at the highest achievable position. */
 		TOP(45),
 
-		/** Shoulder is in position to intake and outtake pieces from/to the floor.  Starting position. */
-		FLOOR(20),
+		/** Shoulder is in position to intake from the substation or score in the upper nodes. */
+		RAISED(40),
 
-		/** Shoulder is fully down. **/
-		BOTTOM(15);
+		/** Shoulder is in position to intake and outtake pieces from/to the floor. */
+		FLOOR(10),
+
+		/** Shoulder is fully down.  Starting position. **/
+		BOTTOM(0);
 
 		private final double angle;
 
 		Position(double angle) {
 			this.angle = angle;
 		}
 
-		private double getAngle() {
+		public double getAngle() {
 			return angle;
 		}
 	}
@@ -78,12 +83,15 @@ public Shoulder() {
 			throw new IllegalStateException("Motor are not CANSparkMaxes!");
 		}
 
+		halLeftMotor.setNeutralMode(NeutralMode.Brake);
+		halRightMotor.setNeutralMode(NeutralMode.Brake);
+
 		leftMotor = (CANSparkMax) halLeftMotor;
 		rightMotor = (CANSparkMax) halRightMotor;
 
 		rightMotor.follow(leftMotor, true /* invert */);
 
-		leftMotor.getEncoder().setPosition(EncoderUtils.elevatorHeightToRotations(Position.BOTTOM.getAngle()));
+		leftMotor.getEncoder().setPosition(EncoderUtils.shoulderDegreesToRotations(Position.BOTTOM.getAngle()));
 
 		pidController = leftMotor.getPIDController();
 		pidController.setFeedbackDevice(leftMotor.getEncoder());
@@ -168,10 +176,11 @@ public void rotate(double angle) {
 		SmartDashboard.putNumber("[SHOULDER] ff", ff);
 		SmartDashboard.putNumber("[SHOULDER] reference", angle);
 
-		pidController.setOutputRange(-1, 1);
+		pidController.setOutputRange(-0.4, 0.4);
 
 		// convert the desired target degrees to rotations
 		double rotations = EncoderUtils.shoulderDegreesToRotations(angle);
+		SmartDashboard.putNumber("[SHOULDER] Setpoint", rotations);
 
 		// set the reference point for the wrist
 		pidController.setReference(rotations, ControlType.kPosition, 0, ff);
@@ -182,6 +191,8 @@ public void run() {
 		if (rateLimiter.next()) {
 			SmartDashboard.putNumber("[SHOULDER] Angle", getAngle());
 			SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations());
+			SmartDashboard.putNumber("[SHOULDER] Left Effort", leftMotor.getOutputCurrent());
+			SmartDashboard.putNumber("[SHOULDER] Right Effort", rightMotor.getOutputCurrent());
 		}
 	}
 }

src/main/java/com/team766/robot/mechanisms/Wrist.java

@@ -29,15 +29,15 @@ public class Wrist extends Mechanism {
 	public enum Position {
 
 		/** Wrist is in top position.  Starting position. */
-		TOP(-135),
+		TOP(-180),
 		/** Wrist is in the position for moving around the field. */ 
-		RETRACTED(-120.0),
+		RETRACTED(-175.0),
 		/** Wrist is level with ground. */
-		LEVEL(0.0),
-		HIGH_NODE(-30),
+		LEVEL(-65),
+		HIGH_NODE(-20),
 		MID_NODE(-25.5),
-		HUMAN_CONES(-17.5),
-		HUMAN_CUBES(-17.5),
+		HUMAN_CONES(-4),
+		HUMAN_CUBES(-8),
 		/** Wrist is fully down. **/
 		BOTTOM(60);
 

src/main/java/com/team766/robot/procedures/ExtendWristvatorToHigh.java

@@ -1,11 +1,12 @@
 package com.team766.robot.procedures;
 
 import com.team766.robot.mechanisms.Elevator;
+import com.team766.robot.mechanisms.Shoulder;
 import com.team766.robot.mechanisms.Wrist;
 
 public class ExtendWristvatorToHigh extends MoveWristvator {
 
 	public ExtendWristvatorToHigh() {
-		super(Elevator.Position.HIGH, Wrist.Position.HIGH_NODE);
+		super(Shoulder.Position.RAISED, Elevator.Position.HIGH, Wrist.Position.HIGH_NODE);
 	}
 }

src/main/java/com/team766/robot/procedures/ExtendWristvatorToHuman.java

@@ -1,13 +1,15 @@
 package com.team766.robot.procedures;
 
 import com.team766.robot.mechanisms.Elevator;
+import com.team766.robot.mechanisms.Shoulder;
 import com.team766.robot.mechanisms.Wrist;
 import com.team766.robot.mechanisms.Intake.GamePieceType;
 
 public class ExtendWristvatorToHuman extends MoveWristvator {
 
 	public ExtendWristvatorToHuman(GamePieceType gamePieceType) {
-		super(gamePieceType == GamePieceType.CONE
+		super(Shoulder.Position.RAISED, 
+				gamePieceType == GamePieceType.CONE
 				? Elevator.Position.HUMAN_CONES
 				: Elevator.Position.HUMAN_CUBES, 
 				gamePieceType == GamePieceType.CONE

src/main/java/com/team766/robot/procedures/ExtendWristvatorToLow.java

@@ -1,11 +1,12 @@
 package com.team766.robot.procedures;
 
 import com.team766.robot.mechanisms.Elevator;
+import com.team766.robot.mechanisms.Shoulder;
 import com.team766.robot.mechanisms.Wrist;
 
 public class ExtendWristvatorToLow extends MoveWristvator {
 
 	public ExtendWristvatorToLow() {
-		super(Elevator.Position.LOW, Wrist.Position.LEVEL);
+		super(Shoulder.Position.FLOOR, Elevator.Position.LOW, Wrist.Position.LEVEL);
 	}
 }

src/main/java/com/team766/robot/procedures/ExtendWristvatorToMid.java

@@ -1,11 +1,12 @@
 package com.team766.robot.procedures;
 
 import com.team766.robot.mechanisms.Elevator;
+import com.team766.robot.mechanisms.Shoulder;
 import com.team766.robot.mechanisms.Wrist;
 
 public class ExtendWristvatorToMid extends MoveWristvator {
 
 	public ExtendWristvatorToMid() {
-		super(Elevator.Position.MID, Wrist.Position.MID_NODE);
+		super(Shoulder.Position.RAISED, Elevator.Position.MID, Wrist.Position.MID_NODE);
 	}
 }

src/main/java/com/team766/robot/procedures/MoveWristvator.java

@@ -4,13 +4,16 @@
 import com.team766.framework.Procedure;
 import com.team766.robot.Robot;
 import com.team766.robot.mechanisms.Elevator;
+import com.team766.robot.mechanisms.Shoulder;
 import com.team766.robot.mechanisms.Wrist;
 
 public class MoveWristvator extends Procedure {
+	private final Shoulder.Position shoulderSetpoint;
 	private final Elevator.Position elevatorSetpoint;
 	private final Wrist.Position wristSetpoint;
 
-	public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristSetpoint_) {
+	public MoveWristvator(Shoulder.Position shoulderSetpoint_, Elevator.Position elevatorSetpoint_, Wrist.Position wristSetpoint_) {
+		this.shoulderSetpoint = shoulderSetpoint_;
 		this.elevatorSetpoint = elevatorSetpoint_;
 		this.wristSetpoint = wristSetpoint_;
 	}
@@ -19,18 +22,29 @@ public MoveWristvator(Elevator.Position elevatorSetpoint_, Wrist.Position wristS
 	public final void run(Context context) {
 		context.takeOwnership(Robot.wrist);
 		context.takeOwnership(Robot.elevator);
+		context.takeOwnership(Robot.shoulder);
 
 		// Always retract the wrist before moving the elevator.
 		// It might already be retracted, so it's possible that this step finishes instantaneously.
 		Robot.wrist.rotate(Wrist.Position.RETRACTED);
+		// If raising the shoulder, do that before the elevator (else, lower it after the elevator).
+		if (shoulderSetpoint.getAngle() > Robot.shoulder.getAngle()) {
+			Robot.shoulder.rotate(shoulderSetpoint);
+			context.waitFor(() -> Robot.shoulder.isNearTo(shoulderSetpoint));
+		}
 		context.waitFor(() -> Robot.wrist.isNearTo(Wrist.Position.RETRACTED));
 
 		// Move the elevator. Wait until it gets near the target position.
 		Robot.elevator.moveTo(elevatorSetpoint);
 		context.waitFor(() -> Robot.elevator.isNearTo(elevatorSetpoint));
 
+		// If lowering the shoulder, do that after the elevator.
+		if (shoulderSetpoint.getAngle() < Robot.shoulder.getAngle()) {
+			Robot.shoulder.rotate(shoulderSetpoint);
+		}
 		// Lastly, move the wrist.
 		Robot.wrist.rotate(wristSetpoint);
 		context.waitFor(() -> Robot.wrist.isNearTo(wristSetpoint));
+		context.waitFor(() -> Robot.shoulder.isNearTo(shoulderSetpoint));
 	}
 }

src/main/java/com/team766/robot/procedures/RetractWristvator.java

@@ -1,11 +1,12 @@
 package com.team766.robot.procedures;
 
 import com.team766.robot.mechanisms.Elevator;
+import com.team766.robot.mechanisms.Shoulder;
 import com.team766.robot.mechanisms.Wrist;
 
 public class RetractWristvator extends MoveWristvator {
 
 	public RetractWristvator() {
-		super(Elevator.Position.RETRACTED, Wrist.Position.RETRACTED);
+		super(Shoulder.Position.BOTTOM, Elevator.Position.RETRACTED, Wrist.Position.RETRACTED);
 	}
 }