Shoulder works

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

@@ -3,6 +3,7 @@
 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;
@@ -29,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;
 		}
 	}
@@ -80,14 +84,14 @@ public Shoulder() {
 		}
 
 		halLeftMotor.setNeutralMode(NeutralMode.Brake);
-		halRightMotor.setNeutralMode(NeutralMode.Brake);
+		halRightMotor.setNeutralMode(NeutralMode.Coast);
 
 		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());
@@ -172,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);
@@ -186,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,9 +29,9 @@ 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(-65),
 		HIGH_NODE(-20),

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);
 	}
 }