feat(arm): Implement simulated elbow motor.

simgui-ds.json

@@ -98,10 +98,10 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     },
     {
+      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     }
   ]

simgui.json

@@ -48,7 +48,8 @@
           },
           "Setpoint": {
             "open": true
-          }
+          },
+          "open": true
         },
         "Odometry": {
           "Position": {
@@ -79,7 +80,14 @@
           "Vision Pose": {
             "open": true
           }
-        }
+        },
+        "open": true
+      },
+      "arm": {
+        "elbow": {
+          "open": true
+        },
+        "open": true
       },
       "shooter": {
         "beamBreakSensorSim": {

src/main/java/frc/robot/RobotContainer.java

@@ -82,8 +82,6 @@ private void configureBindings() {
 
     operatorController.x().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.DOWN)));
     operatorController.y().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.UP)));
-
-    operatorController.b().whileTrue(arm.driveElbowWithJoystick(operatorController::getLeftY));
   }
 
   /**

src/main/java/frc/robot/arm/Arm.java

@@ -4,13 +4,10 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj2.command.Command;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
-import frc.robot.arm.ArmConstants.ElbowMotorConstants;
 import frc.robot.arm.ElbowMotorIO.ElbowMotorIOValues;
 import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
-import java.util.function.DoubleSupplier;
 
 /** Subsystem class for the arm subsystem. */
 public class Arm extends Subsystem {
@@ -132,16 +129,7 @@ private void setSetpoint(ArmState setpoint) {
     this.setpoint = setpoint;
 
     shoulderMotor.setSetpoint(setpoint.shoulder().position, setpoint.shoulder().velocity);
-    elbowMotor.setSetpoint(setpoint.elbow().position);
+    elbowMotor.setSetpoint(setpoint.elbow().position, setpoint.elbow().velocity);
     // wristMotor.runSetpoint(state.wrist().getRotations());
   }
-
-  public Command driveElbowWithJoystick(DoubleSupplier joystickSupplier) {
-    return run(() ->
-            elbowMotor.setVoltage(
-                joystickSupplier.getAsDouble()
-                    * -1
-                    * Math.abs(ElbowMotorConstants.MAXIMUM_VOLTAGE)))
-        .finallyDo(elbowMotor::stop);
-  }
 }

src/main/java/frc/robot/arm/ArmConstants.java

@@ -11,15 +11,16 @@ public class ArmConstants {
 
   /** Constants for the shoulder motor. */
   public static class ShoulderMotorConstants {
+    /** Shoulder motor CAN. */
     public static final CAN CAN = new CAN(32);
 
     /** Gearing between the soulder motor and the shoulder joint. */
     public static final double GEARING = 51.2;
 
-    /** Moment of intertia of the shoulder, in kilograms meters squared. */
+    /** Moment of inertia of the shoulder, in kilograms meters squared. */
     public static final double MOI = 0.15093;
 
-    /** Minumum angle of the shoulder joint. */
+    /** Minimum angle of the shoulder joint. */
     public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(20);
 
     /** Maximum angle of the shoulder joint. */
@@ -48,10 +49,39 @@ public static class ShoulderMotorConstants {
 
   /** Constants for the elbow motor. */
   public static class ElbowMotorConstants {
-    /** Maximum voltage that can be applied to the elbow motor. */
-    public static final double MAXIMUM_VOLTAGE = 2.0;
+    /** Elbow motor CAN. */
+    public static final CAN CAN = new CAN(32); // TODO
+
+    /** Gearing between the elbow motor and the elbow joint. */
+    public static final double GEARING = 31.37254902;
+
+    /** Moment of inertia of the shoulder, in kilograms meters squared. */
+    public static final double MOI = 0.05235;
+
+    /** Minimum angle of the elbow joint. */
+    public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-90);
+
+    /** Maximum angle of the elbow joint. */
+    public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(180);
 
     /** Elbow pivot to wrist pivot distance in meters. */
     public static final double ELBOW_TO_WRIST_DISTANCE = Units.inchesToMeters(16.825);
+
+    /** Proportional gain in volts per rotation. */
+    public static final double KP = 36.0;
+
+    /** Maximum speed of the shoulder joint in rotations per second. */
+    public static final double MAXIMUM_SPEED = 2.0;
+
+    /** Maximum acceleration of the shoulder joint in rotations per second per second. */
+    public static final double MAXIMUM_ACCELERATION =
+        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3);
+
+    /** Maximum speed and acceleration of the shoulder joint. */
+    public static final TrapezoidProfile.Constraints CONSTRAINTS =
+        new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION);
+
+    /** Motion profile of the shoulder joint using constraints. */
+    public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS);
   }
 }

src/main/java/frc/robot/arm/ArmState.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import frc.robot.RobotConstants;
+import frc.robot.arm.ArmConstants.ElbowMotorConstants;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
 import java.util.Objects;
 
@@ -12,13 +13,13 @@ public record ArmState(State shoulder, State elbow, State wrist) {
   public static ArmState UP =
       new ArmState(
           ShoulderMotorConstants.MAXIMUM_ANGLE,
-          Rotation2d.fromDegrees(0),
+          ElbowMotorConstants.MAXIMUM_ANGLE,
           Rotation2d.fromDegrees(0));
 
   public static ArmState DOWN =
       new ArmState(
           ShoulderMotorConstants.MINIMUM_ANGLE,
-          Rotation2d.fromDegrees(0),
+          ElbowMotorConstants.MINIMUM_ANGLE,
           Rotation2d.fromDegrees(0));
 
   /**
@@ -73,6 +74,10 @@ public ArmState nextSetpoint(ArmState goal) {
         ShoulderMotorConstants.MOTION_PROFILE.calculate(
             RobotConstants.PERIODIC_DURATION, this.shoulder, goal.shoulder);
 
-    return withShoulder(nextShoulderState);
+    State nextElbowState =
+        ElbowMotorConstants.MOTION_PROFILE.calculate(
+            RobotConstants.PERIODIC_DURATION, this.elbow, goal.elbow);
+
+    return new ArmState(nextShoulderState, nextElbowState, new State(0.0, 0.0));
   }
 }

src/main/java/frc/robot/arm/ElbowMotorIO.java

@@ -33,7 +33,7 @@ public static class ElbowMotorIOValues {
    *
    * @param positionRotations the elbow motor's setpoint.
    */
-  public void setSetpoint(double positionRotations);
+  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond);
 
   // TODO Remove, only for characterization
   /**

src/main/java/frc/robot/arm/ElbowMotorIOSim.java

@@ -1,23 +1,84 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.lib.ArmFeedforwardCalculator;
+import frc.robot.RobotConstants;
+import frc.robot.arm.ArmConstants.ElbowMotorConstants;
+
 /** Simulated elbow motor. */
 public class ElbowMotorIOSim implements ElbowMotorIO {
 
+  private final DCMotor motor = DCMotor.getNEO(1);
+
+  private final SingleJointedArmSim singleJointedArmSim;
+
+  private final PIDController feedback;
+
+  private final ArmFeedforward feedforward;
+
+  /** Creates a new simulated elbow motor. */
+  public ElbowMotorIOSim() {
+    singleJointedArmSim =
+        new SingleJointedArmSim(
+            motor,
+            ElbowMotorConstants.GEARING,
+            ElbowMotorConstants.MOI,
+            ElbowMotorConstants.ELBOW_TO_WRIST_DISTANCE,
+            ElbowMotorConstants.MINIMUM_ANGLE.getRadians(),
+            ElbowMotorConstants.MAXIMUM_ANGLE.getRadians(),
+            true,
+            0.0);
+
+    feedback = new PIDController(ElbowMotorConstants.KP, 0, 0);
+
+    feedforward =
+        new ArmFeedforward(
+            0,
+            ArmFeedforwardCalculator.calculateArmGravityCompensation(
+                Rotation2d.fromDegrees(-54.873534), 0.152426),
+            0);
+  }
+
   @Override
   public void configure() {}
 
   @Override
-  public void update(ElbowMotorIOValues values) {}
+  public void update(ElbowMotorIOValues values) {
+    singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION);
+
+    values.positionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads());
+    values.currentAmps = singleJointedArmSim.getCurrentDrawAmps();
+  }
 
   @Override
-  public void setPosition(double positionRotations) {}
+  public void setPosition(double positionRotations) {
+    singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0);
+  }
 
   @Override
-  public void setSetpoint(double positionRotations) {}
+  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
+    double measuredPositionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads());
+
+    double feedbackVolts = feedback.calculate(measuredPositionRotations, positionRotations);
+
+    double feedforwardVolts =
+        feedforward.calculate(measuredPositionRotations, velocityRotationsPerSecond);
+
+    setVoltage(feedbackVolts + feedforwardVolts);
+  }
 
   @Override
-  public void setVoltage(double volts) {}
+  public void setVoltage(double volts) {
+    singleJointedArmSim.setInputVoltage(volts);
+  }
 
   @Override
-  public void stop() {}
+  public void stop() {
+    setVoltage(0.0);
+  }
 }

src/main/java/frc/robot/arm/ShoulderMotorIOSim.java

@@ -13,6 +13,8 @@
 /** Simulated shoulder motor. */
 public class ShoulderMotorIOSim implements ShoulderMotorIO {
 
+  private final DCMotor motor = DCMotor.getNEO(1);
+
   private final SingleJointedArmSim singleJointedArmSim;
 
   private final PIDController feedback;
@@ -23,14 +25,14 @@ public class ShoulderMotorIOSim implements ShoulderMotorIO {
   public ShoulderMotorIOSim() {
     singleJointedArmSim =
         new SingleJointedArmSim(
-            DCMotor.getNEO(1),
+            motor,
             ShoulderMotorConstants.GEARING,
             ShoulderMotorConstants.MOI,
             ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
             ShoulderMotorConstants.MINIMUM_ANGLE.getRadians(),
             ShoulderMotorConstants.MAXIMUM_ANGLE.getRadians(),
             true,
-            Units.degreesToRadians(90));
+            0.0);
 
     feedback = new PIDController(ShoulderMotorConstants.KP, 0, 0);