resolved merge conflicts

SciBorgs · Jan 11, 2025 · 3a02cb3 · 3a02cb3
2 parents 17cebc0 + a93732b
commit 3a02cb3
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 6 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,6 +81,9 @@
     }
   },
   "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
     "Retained Values": {
       "open": false
     },
@@ -181,7 +190,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 338,
+          "height": 332,
           "series": [
             {
               "color": [
@@ -200,6 +209,15 @@
                 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,5 +1,7 @@
 package org.sciborgs1155.robot.arm;
 
+import static edu.wpi.first.units.Units.Centimeters;
+import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
@@ -9,6 +11,11 @@
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 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;
@@ -30,6 +37,19 @@ public class Arm extends SubsystemBase implements Logged, AutoCloseable {
               MAX_VELOCITY.in(RadiansPerSecond), MAX_ACCEl.in(RadiansPerSecondPerSecond)));
   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.in(Degrees),
+              10,
+              new Color8Bit(Color.kSkyBlue)));
+
   /**
    * Returns a new {@link Arm} subsystem, which will have real hardware if the robot is real, and
    * simulated if it isn't.
@@ -57,6 +77,7 @@ private Arm(ArmIO hardware) {
    */
   @Log.NT
   public double position() {
+    armGUI.setAngle(Math.toDegrees(hardware.position()));
     return hardware.position();
   }
 

diff --git a/src/main/java/org/sciborgs1155/robot/arm/SimArm.java b/src/main/java/org/sciborgs1155/robot/arm/SimArm.java
@@ -23,6 +23,7 @@ public class SimArm implements ArmIO {
 
   @Override
   public double position() {
+    simulation.update(Constants.PERIOD.in(Seconds));
     return simulation.getAngleRads();
   }