feat(arm): Add arm visualization.

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

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

@@ -1,9 +1,11 @@
 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.ArmMechanism;
 import frc.robot.auto.Auto;
 import frc.robot.climber.Climber;
 import frc.robot.intake.Intake;
@@ -56,6 +58,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. */

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

@@ -55,6 +55,8 @@ public static Arm getInstance() {
   @Override
   public void periodic() {
     shoulderMotor.update(shoulderMotorValues);
+
+    ArmMechanism.getInstance().setState(getState());
   }
 
   @Override
@@ -103,14 +105,14 @@ public Command hold() {
   public Command driveShoulderWithJoystick(DoubleSupplier joystickSupplier) {
     return run(() ->
             shoulderMotor.setVoltage(
-                joystickSupplier.getAsDouble() * ShoulderMotorConstants.MAXIMUM_VOLTAGE))
+                joystickSupplier.getAsDouble() * -1 * Math.abs(ShoulderMotorConstants.MAXIMUM_VOLTAGE)))
         .finallyDo(shoulderMotor::stop);
   }
 
   public Command driveElbowWithJoystick(DoubleSupplier joystickSupplier) {
     return run(() ->
             elbowMotor.setVoltage(
-                joystickSupplier.getAsDouble() * ElbowMotorConstants.MAXIMUM_VOLTAGE))
+                joystickSupplier.getAsDouble() * -1 * Math.abs(ElbowMotorConstants.MAXIMUM_VOLTAGE)))
         .finallyDo(shoulderMotor::stop);
   }
 }

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

@@ -1,5 +1,6 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.util.Units;
 import frc.lib.CAN;
 
 /** Constants for the arm subsystem. */
@@ -12,19 +13,19 @@ public static class ShoulderMotorConstants {
     /** Gearing between the soulder motor and the shoulder joint. */
     public static final double GEARING = 51.2;
 
-    /** If true, invert the shoulder motor. */
-    public static final boolean MOTOR_IS_INVERTED = true;
-
-    /** If true, invert the shoulder motor's encoder. */
-    public static final boolean ENCODER_IS_INVERTED = true;
-
     /** Maximum voltage that can be applied to the shoulder motor. */
     public static final double MAXIMUM_VOLTAGE = 2.0;
+
+    /** Shoulder pivot to elbow pivot distance in meters. */
+    public static final double SHOULDER_TO_ELBOW_DISTANCE = Units.inchesToMeters(16.775);
   }
 
   /** 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();
   }
 
   /**

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

@@ -0,0 +1,48 @@
+package frc.robot.arm;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.arm.ArmConstants.ElbowMotorConstants;
+import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+
+/** Helper class for rendering the arm using WPILib mechanisms. */
+public class ArmMechanism {
+
+  private static ArmMechanism instance = null;
+
+  private final Mechanism2d mechanism;
+
+  private final MechanismRoot2d root;
+
+  private final MechanismLigament2d shoulder, elbow;
+
+  private final double THICKNESS = Units.inchesToMeters(2) * 100;
+
+  private ArmMechanism() {
+    mechanism = new Mechanism2d(1, 1);
+    root = mechanism.getRoot("arm", 0.15, 0);
+    shoulder = root.append(new MechanismLigament2d("shoulder", ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE, 90, THICKNESS, new Color8Bit(Color.kOrange)));
+    elbow = shoulder.append(new MechanismLigament2d("elbow", ElbowMotorConstants.ELBOW_TO_WRIST_DISTANCE, 90, THICKNESS, new Color8Bit(Color.kGreen)));
+  }
+
+  public static ArmMechanism getInstance() {
+    if (instance == null) {
+      instance = new ArmMechanism();
+    }
+
+    return instance;
+  }
+
+  public Mechanism2d getMechanism() {
+    return mechanism;
+  }
+
+  public void setState(ArmState state) {
+    shoulder.setAngle(state.shoulder());
+    elbow.setAngle(state.elbow().minus(state.shoulder()));
+  }
+}

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

@@ -1,7 +1,7 @@
 package frc.robot.arm;
 
 /** Simulated elbow motor. */
-public class ElbowMotorIOSim  implements ElbowMotorIO {
+public class ElbowMotorIOSim implements ElbowMotorIO {
 
   @Override
   public void configure() {}

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

@@ -0,0 +1,60 @@
+package frc.robot.arm;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.robot.RobotConstants;
+import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+
+/** Simulated shoulder motor. */
+public class ShoulderMotorIOSim implements ShoulderMotorIO {
+
+  private final SingleJointedArmSim singleJointedArmSim;
+
+  /** Creates a new simulated shoulder motor. */
+  public ShoulderMotorIOSim() {
+    singleJointedArmSim =
+        new SingleJointedArmSim(
+            DCMotor.getNEO(1),
+            ShoulderMotorConstants.GEARING,
+            0.1,
+            ShoulderMotorConstants.SHOULDER_TO_ELBOW_DISTANCE,
+            Units.degreesToRadians(10),
+            Units.degreesToRadians(90),
+            false,
+            Units.degreesToRadians(90));
+  }
+
+  @Override
+  public void configure() {
+  }
+
+  @Override
+  public void update(ShoulderMotorIOValues values) {
+    singleJointedArmSim.update(RobotConstants.PERIODIC_DURATION);
+
+    values.positionRotations = Units.radiansToRotations(singleJointedArmSim.getAngleRads());
+    values.currentAmps = singleJointedArmSim.getCurrentDrawAmps();
+  }
+
+  @Override
+  public void setPosition(double positionRotations) {
+    singleJointedArmSim.setState(Units.rotationsToRadians(positionRotations), 0.0);
+  }
+
+  @Override
+  public void runSetpoint(double positionRotations) {
+    //setPosition(positionRotations);
+  }
+
+  // TODO Remove, only for characterization
+  @Override
+  public void setVoltage(double volts) {
+    singleJointedArmSim.setInputVoltage(volts);
+  }
+
+  @Override
+  public void stop() {
+    setVoltage(0);
+  }
+}

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

@@ -16,6 +16,7 @@ public class ShoulderMotorIOSparkMax implements ShoulderMotorIO {
 
   /** Feedback controller for shoulder position. */
   private final PIDController feedback = new PIDController(0, 0, 0);
+
   // private final ProfiledPIDController feedback =
   //     new ProfiledPIDController(0, 0, 0, new Constraints(0, 0));
 
@@ -32,10 +33,6 @@ public void configure() {
     Configurator.configureREV(sparkMax::restoreFactoryDefaults);
 
     Configurator.configureREV(() -> sparkMax.setIdleMode(IdleMode.kBrake));
-
-    sparkMax.setInverted(ShoulderMotorConstants.MOTOR_IS_INVERTED);
-
-    Configurator.configureREV(() -> sparkMax.getEncoder().setInverted(ShoulderMotorConstants.ENCODER_IS_INVERTED));
   }
 
   @Override

src/main/java/frc/robot/intake/IntakeFactory.java

@@ -1,7 +1,5 @@
 package frc.robot.intake;
 
-import frc.robot.Robot;
-
 /** Helper class for creating hardware for the intake subsystem. */
 public class IntakeFactory {
 
@@ -11,7 +9,7 @@ public class IntakeFactory {
    * @return a roller motor.
    */
   public static RollerMotorIO createRollerMotor() {
-    //if (Robot.isReal()) return new RollerMotorIOSparkMax();
+    // if (Robot.isReal()) return new RollerMotorIOSparkMax();
 
     return new RollerMotorIOSim();
   }

src/main/java/frc/robot/lights/LightsFactory.java

@@ -1,7 +1,5 @@
 package frc.robot.lights;
 
-import frc.robot.Robot;
-
 /** Helper class for creating hardware for the lights subsystem. */
 public class LightsFactory {
 
@@ -11,7 +9,7 @@ public class LightsFactory {
    * @return an LED controller.
    */
   public static LEDControllerIO createLEDController() {
-    //if (Robot.isReal()) return new LEDControllerIOCANdle();
+    // if (Robot.isReal()) return new LEDControllerIOCANdle();
 
     return new LEDControllerIOSim();
   }

src/main/java/frc/robot/odometry/OdometryFactory.java

@@ -2,7 +2,6 @@
 
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import frc.lib.CAN;
-import frc.robot.Robot;
 
 /** Helper class for creating hardware for the odometry subsystem. */
 public class OdometryFactory {
@@ -13,7 +12,7 @@ public class OdometryFactory {
    * @return a gyroscope.
    */
   public static GyroscopeIO createGyroscope(CAN gyroscopeCAN, Odometry odometry) {
-    //if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN);
+    // if (Robot.isReal()) return new GyroscopeIOPigeon2(gyroscopeCAN);
 
     return new GyroscopeIOSim(odometry);
   }

src/main/java/frc/robot/shooter/ShooterFactory.java

@@ -22,7 +22,7 @@ public static BeamBreakSensorIO createBeamBreakSensor() {
    * @return a serializer motor.
    */
   public static SerializerMotorIO createSerializerMotor() {
-    //if (Robot.isReal()) return new SerializerMotorIOSparkMax();
+    // if (Robot.isReal()) return new SerializerMotorIOSparkMax();
 
     return new SerializerMotorIOSim();
   }
@@ -33,7 +33,7 @@ public static SerializerMotorIO createSerializerMotor() {
    * @return a flywheel motor.
    */
   public static FlywheelMotorIO createFlywheelMotor() {
-    //if (Robot.isReal()) return new FlywheelMotorIOSparkMax(); // TODO
+    // if (Robot.isReal()) return new FlywheelMotorIOSparkMax(); // TODO
 
     return new FlywheelMotorIOSim();
   }

src/main/java/frc/robot/swerve/SwerveFactory.java

@@ -47,7 +47,8 @@ public static CANcoderConfiguration createAzimuthEncoderConfig() {
    */
   public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
     // if (Robot.isReal())
-    //   return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(), config.moduleCAN().azimuth());
+    //   return new SteerMotorIOTalonFXPIDF(config.moduleCAN().steer(),
+    // config.moduleCAN().azimuth());
 
     return new SteerMotorIOSim();
   }
@@ -58,7 +59,7 @@ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
    * @return a drive motor.
    */
   public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) {
-    //if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive());
+    // if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive());
 
     return new DriveMotorIOSim();
   }