From ea86fd2f2d0fa551ba72522524a5f1d76dd66543 Mon Sep 17 00:00:00 2001
From: Dean Brettle <dean@brettle.com>
Date: Wed, 3 Apr 2024 13:39:57 -0700
Subject: [PATCH] Add simple simulation of arm going to goal position.

No physics, just instantaneously moves to the goal. Useful for auto testing.
---
 build.gradle                                      |  2 +-
 simgui-ds.json                                    | 10 ++++++----
 simgui.json                                       |  8 ++++++++
 .../java/org/carlmontrobotics/subsystems/Arm.java | 15 +++++++++++++++
 4 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/build.gradle b/build.gradle
index 0243b91a..a3cab5c4 100644
--- a/build.gradle
+++ b/build.gradle
@@ -79,7 +79,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    implementation "com.github.deepbluerobotics:lib199:0e5932a4219f3439ef2be3981a9189baf777f61e"
+    implementation "com.github.deepbluerobotics:lib199:0ce129844b9b918dad0207c0e1020fc26588f461"
 }
 
 test {
diff --git a/simgui-ds.json b/simgui-ds.json
index 9c0ce4ad..083ab12f 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -52,7 +52,7 @@
           "incKey": 75
         }
       ],
-      "axisCount": 2,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         77,
@@ -60,7 +60,7 @@
         46,
         47
       ],
-      "povCount": 0
+      "povCount": 1
     },
     {
       "axisConfig": [
@@ -93,10 +93,12 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "Keyboard0",
+      "name": "Driver"
     },
     {
-      "guid": "Keyboard1"
+      "guid": "Keyboard1",
+      "name": "Manipulator"
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
index 9b3270e4..e6f7c67b 100644
--- a/simgui.json
+++ b/simgui.json
@@ -6,6 +6,11 @@
       }
     },
     "Other Devices": {
+      "AbsoluteEncoder[13]": {
+        "header": {
+          "open": true
+        }
+      },
       "CANCoder (v6)[2]": {
         "header": {
           "open": true
@@ -63,6 +68,9 @@
   "NetworkTables": {
     "transitory": {
       "SmartDashboard": {
+        "Arm": {
+          "open": true
+        },
         "BL": {
           "open": true
         },
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index 4e600b5c..12e1246a 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -19,6 +19,7 @@
 import com.revrobotics.SparkAbsoluteEncoder;
 import com.revrobotics.SparkPIDController;
 
+import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -30,9 +31,11 @@
 import edu.wpi.first.units.Voltage;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -76,6 +79,9 @@ public class Arm extends SubsystemBase {
 
     private ShuffleboardTab sysIdTab = Shuffleboard.getTab("arm SysID");
     private boolean setPIDOff; 
+
+    private SimDouble rotationsSim;
+
     public Arm() {
         // weird math stuff
         armMotorMaster.setInverted(MOTOR_INVERTED_MASTER);
@@ -144,6 +150,9 @@ public Arm() {
         armMotorMaster.setSmartCurrentLimit(80);
         armMotorFollower.setSmartCurrentLimit(80);
 
+        if (RobotBase.isSimulation()) {
+            rotationsSim = new SimDeviceSim("AbsoluteEncoder", ARM_MOTOR_PORT_MASTER).getDouble("rotations");
+        }
     }
 
     public void setBooleanDrive(boolean climb) {
@@ -420,4 +429,10 @@ public void setDefaultCommand(TeleopArm teleopArm, Object object) {
         // TODO Auto-generated method stub
         throw new UnsupportedOperationException("Unimplemented method 'setDefaultCommand'");
     }
+
+    @Override
+    public void simulationPeriodic() {
+        // Fake goaling to the goal instantaneously
+        rotationsSim.set((goalState.position + armMasterEncoder.getZeroOffset()) / armMasterEncoder.getPositionConversionFactor());
+    }
 }