diff --git a/src/main/java/com/team766/robot/constants/ConfigConstants.java b/src/main/java/com/team766/robot/constants/ConfigConstants.java index 793fa3f..89c89f0 100644 --- a/src/main/java/com/team766/robot/constants/ConfigConstants.java +++ b/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"; } diff --git a/src/main/java/com/team766/robot/mechanisms/Elevator.java b/src/main/java/com/team766/robot/mechanisms/Elevator.java index e62a77f..0e047d4 100644 --- a/src/main/java/com/team766/robot/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/mechanisms/Elevator.java @@ -2,7 +2,6 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.SparkMaxPIDController.AccelStrategy; import com.team766.config.ConfigFileReader; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; @@ -14,12 +13,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 +64,6 @@ private double getHeight() { private final ValueProvider minOutputVelocity; private final ValueProvider maxAccel; - private final RateLimiter rateLimiter = new RateLimiter(1.0 /* seconds */); /** @@ -71,7 +73,7 @@ public Elevator() { MotorController halLeftMotor = RobotProvider.instance.getMotor(ELEVATOR_LEFT_MOTOR); MotorController halRightMotor = RobotProvider.instance.getMotor(ELEVATOR_RIGHT_MOTOR); - if (!((halLeftMotor instanceof CANSparkMax)&&(halRightMotor instanceof CANSparkMax))) { + if (!((halLeftMotor instanceof CANSparkMax) && (halRightMotor instanceof CANSparkMax))) { log(Severity.ERROR, "Motors are not CANSparkMaxes!"); throw new IllegalStateException("Motor are not CANSparkMaxes!"); } @@ -79,7 +81,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())); @@ -132,7 +134,6 @@ public void nudgeUp() { double height = getHeight(); // NOTE: this could artificially limit nudge range double targetHeight = Math.min(height + NUDGE_INCREMENT, Position.EXTENDED.getHeight()); - System.err.println("Target: " + targetHeight); moveTo(targetHeight); } diff --git a/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java b/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java index deee0bb..1525fa0 100644 --- a/src/main/java/com/team766/robot/mechanisms/EncoderUtils.java +++ b/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.); } @@ -35,7 +33,7 @@ public static double wristRotationsToDegrees(double rotations) { */ public static double elevatorHeightToRotations(double height) { // height * net gear ratio * (rotations / height) - return height * (36./12.) * (1./(1.641 * Math.PI)); + return height * (36. / 12.) * (1. / (1.641 * Math.PI)); } /** @@ -43,8 +41,23 @@ 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.); + 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.); } /** @@ -55,18 +68,18 @@ public static double elevatorRotationsToHeight(double rotations) { * @return */ public static double lawOfCosines(double side1, double side2, double angle) { - double side3Squared = (Math.pow(side1,2.0)+Math.pow(side2,2.0)-2*side1*side2*Math.cos(Math.toRadians(angle))); + double side3Squared = (Math.pow(side1, 2.0) + Math.pow(side2, 2.0) - (2 * side1 * side2 * Math.cos(Math.toRadians(angle)))); return Math.sqrt(side3Squared); } public static double lawOfSines(double side1, double angle1, double side2) { - return Math.asin(side2*Math.sin(angle1)/side1); + return Math.asin(side2 * Math.sin(angle1) / side1); } public static double clampValueToRange(double value, double min, double max) { - if (value > max){ + if (value > max) { value = max; - } else if (value < min){ + } else if (value < min) { value = min; } return value; diff --git a/src/main/java/com/team766/robot/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/mechanisms/Shoulder.java new file mode 100644 index 0000000..6378390 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Shoulder.java @@ -0,0 +1,187 @@ +package com.team766.robot.mechanisms; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +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 pGain; + private final ValueProvider iGain; + private final ValueProvider dGain; + private final ValueProvider ffGain; + private final ValueProvider maxVelocity; + private final ValueProvider minOutputVelocity; + private final ValueProvider 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.min(angle + NUDGE_INCREMENT, Position.TOP.getAngle()); + + rotate(targetAngle); + } + + public void nudgeDown() { + System.err.println("Nudging down."); + double angle = getAngle(); + double targetAngle = Math.max(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()); + } + } +} diff --git a/src/main/java/com/team766/robot/mechanisms/Wrist.java b/src/main/java/com/team766/robot/mechanisms/Wrist.java index c8aa68c..ac6a258 100644 --- a/src/main/java/com/team766/robot/mechanisms/Wrist.java +++ b/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. */