Merge pull request #21 from SciBorgs/drive

Drive
SciBorgs · Jan 19, 2024 · 862a01c · 862a01c
2 parents 0ea410a + b95c3e1
commit 862a01c
Show file tree

Hide file tree

Showing 17 changed files with 1,210 additions and 45 deletions.
diff --git a/.Glass/glass.json b/.Glass/glass.json
@@ -0,0 +1,58 @@
+{
+  "NetworkTables": {
+    "transitory": {
+      "Robot": {
+        "drive": {
+          "frontRight": {
+            "SwerveModulePosition##v_/Robot/drive/frontRight/position": {
+              "Rotation2d##v_angle": {
+                "open": true
+              },
+              "open": true
+            },
+            "SwerveModuleState##v_/Robot/drive/frontRight/state": {
+              "Rotation2d##v_angle": {
+                "open": true
+              },
+              "open": true
+            }
+          },
+          "open": true,
+          "rearRight": {
+            "SwerveModulePosition##v_/Robot/drive/rearRight/position": {
+              "Rotation2d##v_angle": {
+                "open": true
+              },
+              "open": true
+            },
+            "module": {
+              "open": true
+            },
+            "open": true
+          }
+        },
+        "open": true
+      }
+    },
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/Robot/autos": "String Chooser",
+      "/Robot/drive/field2d": "Field2d",
+      "/Robot/drive/frontLeft/driveFeedback": "PIDController",
+      "/Robot/drive/frontLeft/turnFeedback": "PIDController",
+      "/Robot/drive/frontRight/driveFeedback": "PIDController",
+      "/Robot/drive/frontRight/turnFeedback": "PIDController",
+      "/Robot/drive/rearLeft/driveFeedback": "PIDController",
+      "/Robot/drive/rearLeft/turnFeedback": "PIDController",
+      "/Robot/drive/rearRight/driveFeedback": "PIDController",
+      "/Robot/drive/rearRight/turnFeedback": "PIDController",
+      "/SmartDashboard/drive quasistatic forward": "Command"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)"
+  }
+}
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -4,6 +4,27 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "org.sciborgs1155.robot.Main",
+      "projectName": "Hydrogen"
+    },
+    {
+      "type": "java",
+      "name": "FlexModule",
+      "request": "launch",
+      "mainClass": "org.sciborgs1155.robot.drive.FlexModule",
+      "projectName": "Hydrogen"
+    },
+    {
+      "type": "java",
+      "name": "TalonSwerveModule",
+      "request": "launch",
+      "mainClass": "org.sciborgs1155.robot.drive.TalonSwerveModule",
+      "projectName": "Hydrogen"
+    },
     {
       "type": "java",
       "name": "FailureManagement",

diff --git a/simgui-ds.json b/simgui-ds.json
@@ -1,10 +1,25 @@
 {
+  "FMS": {
+    "window": {
+      "visible": false
+    }
+  },
+  "Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
+  "System Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
         {
-          "decKey": 65,
-          "incKey": 68
+          "decKey": 68,
+          "incKey": 65
         },
         {
           "decKey": 87,
@@ -15,9 +30,14 @@
           "decayRate": 0.0,
           "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 76,
+          "incKey": 74
         }
       ],
-      "axisCount": 3,
+      "axisCount": 6,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -41,16 +61,13 @@
     },
     {
       "axisConfig": [
-        {
-          "decKey": 74,
-          "incKey": 76
-        },
+        {},
         {
           "decKey": 73,
           "incKey": 75
         }
       ],
-      "axisCount": 2,
+      "axisCount": 4,
       "buttonCount": 4,
       "buttonKeys": [
         77,
@@ -88,5 +105,11 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {},
+    {
+      "guid": "Keyboard0"
+    }
   ]
 }
diff --git a/simgui.json b/simgui.json
@@ -1,31 +1,65 @@
 {
+  "HALProvider": {
+    "Other Devices": {
+      "window": {
+        "visible": false
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
       "/Robot/autos": "String Chooser",
-      "/SmartDashboard/PIDController[1]": "PIDController"
+      "/Robot/drive/field2d": "Field2d",
+      "/Robot/drive/frontLeft/driveFeedback": "PIDController",
+      "/Robot/drive/frontLeft/turnFeedback": "PIDController",
+      "/Robot/drive/frontRight/driveFeedback": "PIDController",
+      "/Robot/drive/frontRight/turnFeedback": "PIDController",
+      "/Robot/drive/rearLeft/driveFeedback": "PIDController",
+      "/Robot/drive/rearLeft/turnFeedback": "PIDController",
+      "/Robot/drive/rearRight/driveFeedback": "PIDController",
+      "/Robot/drive/rearRight/turnFeedback": "PIDController",
+      "/SmartDashboard/drive quasistatic forward": "Command"
+    },
+    "windows": {
+      "/Robot/drive/field2d": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/drive quasistatic forward": {
+        "window": {
+          "visible": true
+        }
+      }
     }
   },
   "NetworkTables": {
     "transitory": {
       "Robot": {
-        "open": true
-      },
-      "Shuffleboard": {
-        "open": true
-      },
-      "SmartDashboard": {
-        "Failures": {
-          "open": true,
-          "string[]##v_/SmartDashboard/Failures/warnings": {
+        "drive": {
+          "frontRight": {
+            "SwerveModulePosition##v_/Robot/drive/frontRight/position": {
+              "Rotation2d##v_angle": {
+                "open": true
+              },
+              "open": true
+            },
+            "SwerveModuleState##v_/Robot/drive/frontRight/state": {
+              "Rotation2d##v_angle": {
+                "open": true
+              },
+              "open": true
+            },
+            "driveFeedback": {
+              "open": true
+            },
             "open": true
-          }
+          },
+          "open": true
         },
         "open": true
       }
     }
-  },
-  "NetworkTables Info": {
-    "visible": true
   }
 }
diff --git a/src/main/java/org/sciborgs1155/lib/SparkUtils.java b/src/main/java/org/sciborgs1155/lib/SparkUtils.java
@@ -4,7 +4,6 @@
 
 import com.revrobotics.CANSparkBase;
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
-import com.revrobotics.SparkPIDController;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Time;
 import edu.wpi.first.units.Units;
@@ -109,19 +108,4 @@ public static void configureFrameStrategy(
   public static void configureFollowerFrameStrategy(CANSparkBase spark) {
     configureFrameStrategy(spark, Set.of(), Set.of(), false);
   }
-
-  /**
-   * Enables and sets the minimum and maximum bounds for input wrapping on an onboard Spark Max PID
-   * controller.
-   *
-   * @param controller The onboard PID controller object.
-   * @param min The minimum position input.
-   * @param max The maximum position input.
-   */
-  public static void enableContinuousPIDInput(
-      SparkPIDController controller, double min, double max) {
-    controller.setPositionPIDWrappingEnabled(true);
-    controller.setPositionPIDWrappingMinInput(min);
-    controller.setPositionPIDWrappingMaxInput(max);
-  }
 }
diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java
@@ -5,4 +5,16 @@ public static final class OI {
     public static final int OPERATOR = 0;
     public static final int DRIVER = 1;
   }
+
+  public static final class Drive {
+    public static final int FRONT_LEFT_DRIVE = 11;
+    public static final int REAR_LEFT_DRIVE = 10;
+    public static final int FRONT_RIGHT_DRIVE = 12;
+    public static final int REAR_RIGHT_DRIVE = 13;
+
+    public static final int FRONT_LEFT_TURNING = 15;
+    public static final int REAR_LEFT_TURNING = 14;
+    public static final int FRONT_RIGHT_TURNING = 16;
+    public static final int REAR_RIGHT_TURNING = 17;
+  }
 }
diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java
@@ -10,10 +10,12 @@
 import monologue.Annotations.Log;
 import monologue.Logged;
 import monologue.Monologue;
+import org.littletonrobotics.urcl.URCL;
 import org.sciborgs1155.lib.CommandRobot;
 import org.sciborgs1155.lib.FaultLogger;
 import org.sciborgs1155.robot.Ports.OI;
 import org.sciborgs1155.robot.commands.Autos;
+import org.sciborgs1155.robot.drive.Drive;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -28,9 +30,10 @@ public class Robot extends CommandRobot implements Logged {
   private final CommandXboxController driver = new CommandXboxController(OI.DRIVER);
 
   // SUBSYSTEMS
+  private final Drive drive = Drive.create();
 
   // COMMANDS
-  @Log.NT Autos autos = new Autos();
+  @Log.NT private final Autos autos = new Autos();
 
   /** The robot contains subsystems, OI devices, and commands. */
   public Robot() {
@@ -41,23 +44,28 @@ public Robot() {
 
   /** Configures basic behavior during different parts of the game. */
   private void configureGameBehavior() {
-    if (isSimulation()) {
-      DriverStation.silenceJoystickConnectionWarning(true);
-    }
-
-    // Configure logging with DataLogManager, Monologue, and FailureManagement
+    // Configure logging with DataLogManager, Monologue, FailureManagement, and URCL
     DataLogManager.start();
     Monologue.setupMonologue(this, "/Robot", false, true);
     addPeriodic(Monologue::updateAll, kDefaultPeriod);
     FaultLogger.setupLogging();
     addPeriodic(FaultLogger::update, 1);
+
+    if (isReal()) {
+      URCL.start();
+    } else {
+      DriverStation.silenceJoystickConnectionWarning(true);
+    }
   }
 
   /**
    * Configures subsystem default commands. Default commands are scheduled when no other command is
    * running on a subsystem.
    */
-  private void configureSubsystemDefaults() {}
+  private void configureSubsystemDefaults() {
+    drive.setDefaultCommand(
+        drive.drive(() -> -driver.getLeftX(), () -> -driver.getLeftY(), () -> -driver.getRightX()));
+  }
 
   /** Configures trigger -> command bindings */
   private void configureBindings() {