Merge pull request #15 from Gongoliers/2_8_2024_arm_testing

2/8/2024 - 2/9/2024 shoulder joint implementation

simgui-ds.json

@@ -1,23 +1,30 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
         {
           "decKey": 65,
           "incKey": 68
         },
-        {
-          "decKey": 87,
-          "incKey": 83
-        },
+        {},
         {
           "decKey": 69,
           "decayRate": 0.0,
           "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 83,
+          "incKey": 87
         }
       ],
-      "axisCount": 3,
+      "axisCount": 6,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -90,12 +97,10 @@
     }
   ],
   "robotJoysticks": [
+    {},
     {
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true
-    },
-    {
-      "useGamepad": true
     }
   ]
 }

simgui.json

@@ -5,6 +5,7 @@
       "/Shuffleboard/Auto/Auto Chooser": "String Chooser",
       "/Shuffleboard/Auto/SendableChooser[0]": "String Chooser",
       "/Shuffleboard/Vision/Field": "Field2d",
+      "/SmartDashboard/Arm Mechanism": "Mechanism2d",
       "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": "Field2d"
     },
     "windows": {
@@ -13,6 +14,11 @@
           "visible": true
         }
       },
+      "/SmartDashboard/Arm Mechanism": {
+        "window": {
+          "visible": true
+        }
+      },
       "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": {
         "window": {
           "visible": true
@@ -23,6 +29,15 @@
   "NetworkTables": {
     "transitory": {
       "Shuffleboard": {
+        "Arm": {
+          "Position": {
+            "open": true
+          },
+          "Setpoint": {
+            "open": true
+          },
+          "open": true
+        },
         "Odometry": {
           "Velocity": {
             "open": true

src/main/java/frc/lib/ArmFeedforwardCalculator.java

@@ -0,0 +1,19 @@
+package frc.lib;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/** Helper class to assist with arm feedforward calculations. */
+public class ArmFeedforwardCalculator {
+
+  /**
+   * Calculates the gravity compensation constant for an arm given the voltage applied at an
+   * equilibrium position.
+   *
+   * @param angle the equilibrium position of the arm.
+   * @param volts the voltage applied to the arm to hold it the equilibrium position.
+   * @return the gravity compensation constant for the arm.
+   */
+  public static double calculateArmGravityCompensation(Rotation2d angle, double volts) {
+    return volts / angle.getCos();
+  }
+}

src/main/java/frc/lib/MotionProfileCalculator.java

@@ -0,0 +1,17 @@
+package frc.lib;
+
+/** Helper class to assist with motion profile calculations. */
+public class MotionProfileCalculator {
+
+  /**
+   * Calculates an acceleration using a ramp duration.
+   *
+   * @param maximumSpeed the maximum speed in units per second.
+   * @param desiredRampDurationSeconds the desired duration to ramp from no speed to full speed.
+   * @return the acceleration in units per second per second.
+   */
+  public static double calculateAcceleration(
+      double maximumSpeed, double desiredRampDurationSeconds) {
+    return maximumSpeed / desiredRampDurationSeconds;
+  }
+}

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

@@ -1,9 +1,12 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.Telemetry;
 import frc.robot.arm.Arm;
+import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+import frc.robot.arm.ArmMechanism;
 import frc.robot.auto.Auto;
 import frc.robot.climber.Climber;
 import frc.robot.intake.Intake;
@@ -56,6 +59,8 @@ public static RobotContainer getInstance() {
   private void initializeTelemetry() {
     Telemetry.initializeShuffleboards(
         arm, auto, climber, intake, lights, odometry, shooter, swerve, vision);
+
+    SmartDashboard.putData("Arm Mechanism", ArmMechanism.getInstance().getMechanism());
   }
 
   /** Configures operator controller bindings. */
@@ -75,7 +80,17 @@ private void configureBindings() {
     operatorController.rightBumper().whileTrue(intake.outtake()).whileTrue(shooter.shoot());
     operatorController.rightTrigger().whileTrue(shooter.smartShoot());
 
-    operatorController.a().whileTrue(arm.driveShoulderWithJoystick(operatorController::getLeftY));
+    operatorController
+        .x()
+        .whileTrue(
+            arm.runSetpoint(
+                () -> arm.getState().withShoulder(ShoulderMotorConstants.MINIMUM_ANGLE)));
+    operatorController
+        .y()
+        .whileTrue(
+            arm.runSetpoint(
+                () -> arm.getState().withShoulder(ShoulderMotorConstants.MAXIMUM_ANGLE)));
+
     operatorController.b().whileTrue(arm.driveElbowWithJoystick(operatorController::getLeftY));
   }
 

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

@@ -1,13 +1,13 @@
 package frc.robot.arm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+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.ArmConstants.ShoulderMotorConstants;
 import frc.robot.arm.ElbowMotorIO.ElbowMotorIOValues;
 import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues;
 import java.util.function.DoubleSupplier;
@@ -31,12 +31,16 @@ public class Arm extends Subsystem {
   /** Elbow motor values. */
   private final ElbowMotorIOValues elbowMotorValues = new ElbowMotorIOValues();
 
+  private ArmState setpoint;
+
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
     shoulderMotor = ArmFactory.createShoulderMotor();
     elbowMotor = ArmFactory.createElbowMotor();
 
     setState(new ArmState(Rotation2d.fromDegrees(90), new Rotation2d(), new Rotation2d()));
+
+    setpoint = getState();
   }
 
   /**
@@ -55,17 +59,25 @@ public static Arm getInstance() {
   @Override
   public void periodic() {
     shoulderMotor.update(shoulderMotorValues);
+
+    ArmMechanism.getInstance().setState(getState());
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout shoulder = Telemetry.addColumn(tab, "Shoulder");
+    ShuffleboardLayout position = Telemetry.addColumn(tab, "Position");
 
-    shoulder.addDouble("Position (rot)", () -> shoulderMotorValues.positionRotations);
+    position.addDouble(
+        "Shoulder Position (deg)",
+        () -> Units.rotationsToDegrees(shoulderMotorValues.positionRotations));
+    position.addDouble(
+        "Elbow Position (deg)", () -> Units.rotationsToDegrees(elbowMotorValues.positionRotations));
 
-    ShuffleboardLayout elbow = Telemetry.addColumn(tab, "Elbow");
+    ShuffleboardLayout setpoint = Telemetry.addColumn(tab, "Setpoint");
 
-    elbow.addDouble("Position (rot)", () -> elbowMotorValues.positionRotations);
+    setpoint.addDouble("Shoulder Setpoint (deg)", () -> this.setpoint.shoulder().getDegrees());
+    setpoint.addDouble("Elbow Setpoint (deg)", () -> this.setpoint.elbow().getDegrees());
+    setpoint.addDouble("Wrist Setpoint (deg)", () -> this.setpoint.wrist().getDegrees());
   }
 
   public void setState(ArmState state) {
@@ -86,31 +98,32 @@ public ArmState getState() {
         Rotation2d.fromRotations(0));
   }
 
-  private void runSetpoint(ArmState state) {
-    shoulderMotor.runSetpoint(state.shoulder().getRotations());
-    elbowMotor.runSetpoint(state.elbow().getRotations());
-    // wristMotor.runSetpoint(state.wrist().getRotations());
+  public ArmState getSetpoint() {
+    return setpoint;
   }
 
-  public Command runSetpoint(Supplier<ArmState> stateSupplier) {
-    return run(() -> runSetpoint(stateSupplier.get()));
+  private void setSetpoint(ArmState setpoint) {
+    this.setpoint = setpoint;
+
+    shoulderMotor.setSetpoint(setpoint.shoulder().getRotations());
+    elbowMotor.setSetpoint(setpoint.elbow().getRotations());
+    // wristMotor.runSetpoint(state.wrist().getRotations());
   }
 
-  public Command hold() {
-    return runSetpoint(this::getState);
+  public Command runSetpoint(Supplier<ArmState> setpointSupplier) {
+    return run(() -> setSetpoint(setpointSupplier.get()));
   }
 
-  public Command driveShoulderWithJoystick(DoubleSupplier joystickSupplier) {
-    return run(() ->
-            shoulderMotor.setVoltage(
-                joystickSupplier.getAsDouble() * ShoulderMotorConstants.MAXIMUM_VOLTAGE))
-        .finallyDo(shoulderMotor::stop);
+  public Command hold() {
+    return runOnce(() -> setSetpoint(getState())).andThen(runSetpoint(this::getSetpoint));
   }
 
   public Command driveElbowWithJoystick(DoubleSupplier joystickSupplier) {
     return run(() ->
             elbowMotor.setVoltage(
-                joystickSupplier.getAsDouble() * ElbowMotorConstants.MAXIMUM_VOLTAGE))
-        .finallyDo(shoulderMotor::stop);
+                joystickSupplier.getAsDouble()
+                    * -1
+                    * Math.abs(ElbowMotorConstants.MAXIMUM_VOLTAGE)))
+        .finallyDo(elbowMotor::stop);
   }
 }

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

@@ -1,17 +1,49 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import frc.lib.CAN;
+import frc.lib.MotionProfileCalculator;
+
 /** Constants for the arm subsystem. */
 public class ArmConstants {
 
   /** Constants for the shoulder motor. */
   public static class ShoulderMotorConstants {
-    /** Maximum voltage that can be applied to the shoulder motor. */
-    public static final double MAXIMUM_VOLTAGE = 2.0;
+    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. */
+    public static final double MOI = 0.15093;
+
+    /** Minumum angle of the shoulder joint. */
+    public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(20);
+
+    /** Maximum angle of the shoulder joint. */
+    public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90);
+
+    /** Shoulder pivot to elbow pivot distance in meters. */
+    public static final double SHOULDER_TO_ELBOW_DISTANCE = Units.inchesToMeters(16.775);
+
+    /** 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 = 1.5;
+
+    /** Maximum acceleration of the shoulder joint in rotations per second per second. */
+    public static final double MAXIMUM_ACCELERATION =
+        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3);
   }
 
   /** 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 pivot to wrist pivot distance in meters. */
+    public static final double ELBOW_TO_WRIST_DISTANCE = Units.inchesToMeters(16.825);
   }
 }

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

@@ -13,7 +13,7 @@ public class ArmFactory {
   public static ShoulderMotorIO createShoulderMotor() {
     if (Robot.isReal()) return new ShoulderMotorIOSparkMax();
 
-    return new ShoulderMotorIOSparkMax();
+    return new ShoulderMotorIOSim();
   }
 
   /**
@@ -22,8 +22,6 @@ public static ShoulderMotorIO createShoulderMotor() {
    * @return an elbow motor.
    */
   public static ElbowMotorIO createElbowMotor() {
-    if (Robot.isReal()) return new ElbowMotorIOSparkMax();
-
-    return new ElbowMotorIOSparkMax();
+    return new ElbowMotorIOSim();
   }
 }