Merge pull request #49 from DeepBlueRobotics/add-simple-arm-sim

Add simple simulation of arm going to goal position.
DeepBlueRobotics · Apr 23, 2024 · 61243ea · 61243ea
2 parents 7a9d33e + ff878ce
commit 61243ea
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 4 deletions.
diff --git 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
@@ -52,15 +52,15 @@
           "incKey": 75
         }
       ],
-      "axisCount": 2,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         77,
         44,
         46,
         47
       ],
-      "povCount": 0
+      "povCount": 1
     },
     {
       "axisConfig": [
@@ -97,7 +97,8 @@
       "useGamepad": true
     },
     {
-      "guid": "Keyboard1"
+      "guid": "Keyboard1",
+      "name": "Manipulator"
     }
   ]
 }
diff --git 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
@@ -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);
@@ -145,6 +151,9 @@ public Arm() {
         armMotorFollower.setSmartCurrentLimit(80);
 
         SmartDashboard.putBoolean("arm is at pos", false);
+        if (RobotBase.isSimulation()) {
+            rotationsSim = new SimDeviceSim("AbsoluteEncoder", ARM_MOTOR_PORT_MASTER).getDouble("rotations");
+        }
     }
 
     public void setBooleanDrive(boolean climb) {
@@ -430,4 +439,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());
+    }
 }