add basic implementation for Shoulder code

src/main/java/com/team766/robot/constants/ConfigConstants.java

@@ -36,6 +36,17 @@ private ConfigConstants() {}
 	public static final String ELEVATOR_MAX_VELOCITY = "elevator.sparkPID.maxVelocity";
 	public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.sparkPID.minOutputVelocity";
 	public static final String ELEVATOR_MAX_ACCEL = "elevator.sparkPID.maxAccel";
+
+	// shoulder config values
+	public static final String SHOULDER_LEFT_MOTOR = "shoulder.leftMotor";
+	public static final String SHOULDER_RIGHT_MOTOR = "shoulder.rightMotor";
+	public static final String SHOULDER_PGAIN = "shoulder.sparkPID.pGain";
+	public static final String SHOULDER_IGAIN = "shoulder.sparkPID.iGain";
+	public static final String SHOULDER_DGAIN = "shoulder.sparkPID.dGain";
+	public static final String SHOULDER_FFGAIN = "shoulder.sparkPID.ffGain";
+	public static final String SHOULDER_MAX_VELOCITY = "shoulder.sparkPID.maxVelocity";
+	public static final String SHOULDER_MIN_OUTPUT_VELOCITY = "shoulder.sparkPID.minOutputVelocity";
+	public static final String SHOULDER_MAX_ACCEL = "shoulder.sparkPID.maxAccel";
 }
 
 

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

@@ -14,12 +14,16 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import static com.team766.robot.constants.ConfigConstants.*;
 
+/**
+ * Basic elevator mechanism.  Used in conjunction with the {@link Intake} and {@link Wrist}.
+ * Can be moved up and down as part of teleop or autonomous control to move the {@link Wrist}
+ * and {@link Intake} closer to a game piece or game element (eg node in the 
+ * field, human player station).
+ */
 public class Elevator extends Mechanism {
 	public enum Position {
 
-		// TODO: do we need separate heights for cones vs cubes?
-
-		/** Elevator is fully retracted. */
+		/** Elevator is fully retracted.  Starting position. */
 		RETRACTED(0),
 		/** Elevator is the appropriate height to place game pieces at the low node. */
 		LOW(5), 
@@ -61,7 +65,6 @@ private double getHeight() {
 	private final ValueProvider<Double> minOutputVelocity;
 	private final ValueProvider<Double> maxAccel;
 
-
 	private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */);
 
 	/**
@@ -79,7 +82,7 @@ public Elevator() {
 		leftMotor = (CANSparkMax) halLeftMotor;
 		rightMotor = (CANSparkMax) halRightMotor;
 
-		rightMotor.follow(leftMotor, true);
+		rightMotor.follow(leftMotor, true /* invert */);
 
 		leftMotor.getEncoder().setPosition(EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight()));
 

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

@@ -17,7 +17,6 @@ private EncoderUtils() {
 	 */
 	public static double wristDegreesToRotations(double angle) {
 		// angle * net gear ratio * (rotations / degrees)
-		// FIXME: replace 32 with actual # of teeth
 		return angle * (72. / 10.) * (72. / 20.) * (48. / 24.) * (1. / 360.);
 	}
 
@@ -26,7 +25,6 @@ public static double wristDegreesToRotations(double angle) {
 	 */
 	public static double wristRotationsToDegrees(double rotations) {
 		// rotations * net gear ratio * (degrees / rotations)
-		// FIXME: replace 32 with actual # of teeth
 		return rotations * (10. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.);
 	}
 
@@ -43,10 +41,25 @@ public static double elevatorHeightToRotations(double height) {
 	 */
 	public static double elevatorRotationsToHeight(double rotations) {
 		// rotations * net gear ratio * (height / rotations)
-		// FIXME: everything
 		return rotations * (12./36.) * ((1.641 * Math.PI)/1.);
 	}
 
+	/**
+	 * 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.);
+	}
+
+	/**
+	 * 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.);
+	}
+
 	/**
 	 * Cosine law
 	 * @param side1

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

@@ -0,0 +1,190 @@
+package com.team766.robot.mechanisms;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.SparkMaxAbsoluteEncoder;
+import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+import com.revrobotics.SparkMaxPIDController;
+import com.team766.config.ConfigFileReader;
+import com.team766.framework.Mechanism;
+import com.team766.hal.MotorController;
+import com.team766.hal.RobotProvider;
+import com.team766.library.RateLimiter;
+import com.team766.library.ValueProvider;
+import com.team766.logging.Severity;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import static com.team766.robot.constants.ConfigConstants.*;
+
+/**
+ * Basic shoulder mechanism.  Rotates the {@link Elevator} to different angles, to allow it (and the
+ * attached {@link Wrist} and {@link Intake}) to reach different positions, from the floor to different
+ * heights of nodes.
+ */
+public class Shoulder extends Mechanism {
+
+	/**
+	 * Pre-set positions for the shoulder.
+	 */
+	public enum Position {
+
+		// TODO: adjust these!
+
+		/** Shoulder is in top position. */
+		TOP(45),
+
+		/** Shoulder is in position to intake and outtake pieces from/to the floor.  Starting position. */
+		FLOOR(20),
+
+		/** Shoulder is fully down. **/
+		BOTTOM(15);
+
+		private final double angle;
+
+		Position(double angle) {
+			this.angle = angle;
+		}
+
+		private double getAngle() {
+			return angle;
+		}
+	}
+
+	private static final double NUDGE_INCREMENT = 5.0;
+	private static final double NUDGE_DAMPENER = 0.15;
+
+	private static final double NEAR_THRESHOLD = 5.0;
+
+	private final CANSparkMax leftMotor;
+	private final CANSparkMax rightMotor;
+	private final SparkMaxPIDController pidController;
+	private final ValueProvider<Double> pGain;
+	private final ValueProvider<Double> iGain;
+	private final ValueProvider<Double> dGain;
+	private final ValueProvider<Double> ffGain;
+	private final ValueProvider<Double> maxVelocity;
+	private final ValueProvider<Double> minOutputVelocity;
+	private final ValueProvider<Double> maxAccel;
+
+	private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */);
+
+	/**
+	 * Constructs a new Shoulder.
+	 */
+	public Shoulder() {
+		MotorController halLeftMotor = RobotProvider.instance.getMotor(SHOULDER_LEFT_MOTOR);
+		MotorController halRightMotor = RobotProvider.instance.getMotor(SHOULDER_RIGHT_MOTOR);
+
+		if (!((halLeftMotor instanceof CANSparkMax)&&(halRightMotor instanceof CANSparkMax))) {
+			log(Severity.ERROR, "Motors are not CANSparkMaxes!");
+			throw new IllegalStateException("Motor are not CANSparkMaxes!");
+		}
+
+		leftMotor = (CANSparkMax) halLeftMotor;
+		rightMotor = (CANSparkMax) halRightMotor;
+
+		rightMotor.follow(leftMotor, true /* invert */);
+
+		leftMotor.getEncoder().setPosition(EncoderUtils.elevatorHeightToRotations(Position.BOTTOM.getAngle()));
+
+		pidController = leftMotor.getPIDController();
+		pidController.setFeedbackDevice(leftMotor.getEncoder());
+
+		pGain = ConfigFileReader.getInstance().getDouble(SHOULDER_PGAIN);
+		iGain = ConfigFileReader.getInstance().getDouble(SHOULDER_IGAIN);
+		dGain = ConfigFileReader.getInstance().getDouble(SHOULDER_DGAIN);
+		ffGain = ConfigFileReader.getInstance().getDouble(SHOULDER_FFGAIN);
+		maxVelocity = ConfigFileReader.getInstance().getDouble(SHOULDER_MAX_VELOCITY);
+		minOutputVelocity = ConfigFileReader.getInstance().getDouble(SHOULDER_MIN_OUTPUT_VELOCITY);
+		maxAccel = ConfigFileReader.getInstance().getDouble(SHOULDER_MAX_ACCEL);
+	}
+
+	public double getRotations() {
+		return leftMotor.getEncoder().getPosition();
+	}
+
+	/**
+	 * Returns the current angle of the wrist.
+	 */
+	public double getAngle() {
+		return EncoderUtils.shoulderRotationsToDegrees(leftMotor.getEncoder().getPosition());
+	}
+
+	public boolean isNearTo(Position position) {
+		return isNearTo(position.getAngle());
+	}
+
+	public boolean isNearTo(double angle) {
+		return Math.abs(angle - getAngle()) < NEAR_THRESHOLD;
+	}
+
+	public void nudgeNoPID(double value) {
+		checkContextOwnership();
+		double clampedValue = MathUtil.clamp(value, -1, 1);
+		clampedValue *= NUDGE_DAMPENER; // make nudges less forceful. TODO: make this non-linear
+		leftMotor.set(clampedValue);	
+	}
+
+	public void stopShoulder() {
+		checkContextOwnership();
+		leftMotor.set(0);
+	}
+
+	public void nudgeUp() {
+		System.err.println("Nudging up.");
+		double angle = getAngle();
+		double targetAngle = Math.max(angle - NUDGE_INCREMENT, Position.TOP.getAngle());
+		System.err.println("Target: " + targetAngle);
+
+		rotate(targetAngle);
+	}
+
+	public void nudgeDown() {
+		System.err.println("Nudging down.");
+		double angle = getAngle();
+		double targetAngle = Math.min(angle + NUDGE_INCREMENT, Position.BOTTOM.getAngle());
+		rotate(targetAngle);
+	}
+
+	/** 
+	 * Rotates the wrist to a pre-set {@link Position}.
+	 */
+	public void rotate(Position position) {
+		rotate(position.getAngle());
+	}
+
+	/**
+	 * Starts rotating the wrist to the specified angle.
+	 * NOTE: this method returns immediately.  Check the current wrist position of the wrist
+	 * with {@link #getAngle()}.
+	 */
+	public void rotate(double angle) {
+		checkContextOwnership();
+
+		System.err.println("Setting target angle to " + angle);
+		// set the PID controller values with whatever the latest is in the config
+		pidController.setP(pGain.get());
+		pidController.setI(iGain.get());
+		pidController.setD(dGain.get());
+		// pidController.setFF(ffGain.get());
+		double ff = ffGain.get() * Math.cos(Math.toRadians(angle));
+		SmartDashboard.putNumber("[SHOULDER] ff", ff);
+		SmartDashboard.putNumber("[SHOULDER] reference", angle);
+
+		pidController.setOutputRange(-1, 1);
+
+		// convert the desired target degrees to rotations
+		double rotations = EncoderUtils.shoulderDegreesToRotations(angle);
+
+		// set the reference point for the wrist
+		pidController.setReference(rotations, ControlType.kPosition, 0, ff);
+	}
+
+	@Override
+	public void run() {
+		if (rateLimiter.next()) {
+			SmartDashboard.putNumber("[SHOULDER] Angle", getAngle());
+			SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations());
+		}
+	}
+}

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

@@ -28,8 +28,6 @@ public class Wrist extends Mechanism {
 	 */
 	public enum Position {
 
-		// TODO: adjust these values.
-
 		/** Wrist is in top position.  Starting position. */
 		TOP(-135),
 		/** Wrist is in the position for moving around the field. */