Added Mechanism GUI

SciBorgs · Jan 11, 2025 · a93732b · a93732b
1 parent a0c1414
commit a93732b
Show file tree

Hide file tree

Showing 5 changed files with 110 additions and 22 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -9,11 +9,6 @@
       "visible": true
     }
   },
-  "System Joysticks": {
-    "window": {
-      "visible": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -106,6 +101,13 @@
     }
   ],
   "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    },
+    {
+      "useGamepad": true
+    },
     {
       "guid": "Keyboard0"
     }

diff --git a/simgui.json b/simgui.json
@@ -11,6 +11,7 @@
       "/FMSInfo": "FMSInfo",
       "/Faults/Active Faults": "Alerts",
       "/Faults/Total Faults": "Alerts",
+      "/Robot/arm/armGUICanvas": "Mechanism2d",
       "/Robot/autos": "String Chooser",
       "/Robot/drive/field2d": "Field2d",
       "/Robot/drive/frontLeft/driveFeedback": "PIDController",
@@ -44,6 +45,11 @@
       "/SmartDashboard/turn quasistatic forward": "Command"
     },
     "windows": {
+      "/Robot/arm/armGUICanvas": {
+        "window": {
+          "visible": true
+        }
+      },
       "/Robot/autos": {
         "window": {
           "visible": true
@@ -75,11 +81,17 @@
     }
   },
   "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
     "Retained Values": {
       "open": false
     },
     "transitory": {
       "Robot": {
+        "arm": {
+          "open": true
+        },
         "autos": {
           "open": true
         },
@@ -167,5 +179,49 @@
   },
   "NetworkTables Info": {
     "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 332,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/Robot/arm/position"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/Robot/arm/velocity"
+            },
+            {
+              "color": [
+                0.3333333432674408,
+                0.658823549747467,
+                0.4078431725502014,
+                1.0
+              ],
+              "id": "NT:/Robot/arm/getVoltage"
+            }
+          ]
+        }
+      ]
+    }
   }
 }
diff --git a/src/main/java/org/sciborgs1155/robot/arm/Arm.java b/src/main/java/org/sciborgs1155/robot/arm/Arm.java
@@ -1,28 +1,50 @@
 package org.sciborgs1155.robot.arm;
 
+import static edu.wpi.first.units.Units.Centimeters;
 import static edu.wpi.first.units.Units.Radians;
 import static org.sciborgs1155.robot.arm.ArmConstants.*;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.units.measure.Angle;
+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 edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import monologue.Annotations.Log;
 import monologue.Logged;
 import org.sciborgs1155.robot.Robot;
 
-/** Simple Arm subsystem used for climbing and intaking coral from the ground. */
+/**
+ * Simple Arm subsystem used for climbing and intaking coral from the ground.
+ */
 public class Arm extends SubsystemBase implements Logged, AutoCloseable {
   /** Interface for interacting with the motor itself. */
-  @Log.NT private final ArmIO hardware;
+  @Log.NT
+  private final ArmIO hardware;
 
   private final PIDController feedbackController = new PIDController(kP, kI, kD);
   private final ArmFeedforward feedforwardController = new ArmFeedforward(kS, kG, kV, kA);
 
+  @Log.NT
+  private final Mechanism2d armGUICanvas = new Mechanism2d(60, 60);
+  private final MechanismRoot2d armPivotGUI = armGUICanvas.getRoot("ArmPivot", 30, 30);
+  private final MechanismLigament2d armTowerGUI = armPivotGUI.append(new MechanismLigament2d("ArmTower", 30, -90));
+  private final MechanismLigament2d armGUI = armPivotGUI.append(
+      new MechanismLigament2d(
+          "Arm",
+          ARM_LENGTH.in(Centimeters),
+          STARTING_ANGLE.getDegrees(),
+          10,
+          new Color8Bit(Color.kSkyBlue)));
+
   /**
-   * Returns a new {@link Arm} subsystem, which will have real hardware if the robot is real, and
+   * Returns a new {@link Arm} subsystem, which will have real hardware if the
+   * robot is real, and
    * simulated if it isn't.
    */
   public static Arm create() {
@@ -48,6 +70,7 @@ private Arm(ArmIO hardware) {
    */
   @Log.NT
   public double position() {
+    armGUI.setAngle(Math.toDegrees(hardware.position()));
     return hardware.position();
   }
 
@@ -56,7 +79,7 @@ public double position() {
    */
   @Log.NT
   public double velocity() {
-    return hardware.velocity();
+    return hardware.velocity(); 
   }
 
   /**

diff --git a/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java b/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java
@@ -11,7 +11,7 @@
 /** Constants for the {@link Arm} subsystem. */
 public class ArmConstants {
   // TODO: not tuned (at all)
-  public static final double kP = 1;
+  public static final double kP = 5;
   public static final double kI = 0;
   public static final double kD = 0.01;
 
@@ -33,7 +33,7 @@ public class ArmConstants {
   public static final Distance ARM_LENGTH = Centimeters.of(10);
 
   /** Fully extended. */
-  public static final Angle MIN_ANGLE = Radians.of(0.5);
+  public static final Angle MIN_ANGLE = Radians.of(-3);
 
   /** Fully retracted. */
   public static final Angle MAX_ANGLE = Radians.of(3);

diff --git a/src/main/java/org/sciborgs1155/robot/arm/SimArm.java b/src/main/java/org/sciborgs1155/robot/arm/SimArm.java
@@ -2,25 +2,31 @@
 
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.Seconds;
 import static org.sciborgs1155.robot.arm.ArmConstants.*;
 
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 
 /** Simulated {@link ArmIO} hardware interface. */
 public class SimArm implements ArmIO {
-  private final SingleJointedArmSim simulation =
-      new SingleJointedArmSim(
-          GEARBOX,
-          GEARING,
-          MOI,
-          ARM_LENGTH.in(Meters),
-          MIN_ANGLE.in(Radians),
-          MAX_ANGLE.in(Radians),
-          true,
-          STARTING_ANGLE.getRadians());
+  private final SingleJointedArmSim simulation = new SingleJointedArmSim(
+      GEARBOX,
+      GEARING,
+      MOI,
+      ARM_LENGTH.in(Meters),
+      MIN_ANGLE.in(Radians),
+      MAX_ANGLE.in(Radians),
+      true,
+      STARTING_ANGLE.getRadians());
+
+  private Time previousUpdateTime = Seconds.of(0);
 
   @Override
   public double position() {
+    simulation.update(Timer.getFPGATimestamp() - previousUpdateTime.in(Seconds));
+    previousUpdateTime = Seconds.of(Timer.getFPGATimestamp());
     return simulation.getAngleRads();
   }
 
@@ -40,5 +46,6 @@ public double voltage() {
   }
 
   @Override
-  public void close() {}
+  public void close() {
+  }
 }