Merge remote-tracking branch 'refs/remotes/origin/feat/field_relative…

…' into feat/field_relative
FRC-1721 · Mar 6, 2024 · 7bf923a · 7bf923a
2 parents 6f07eea + 5191242
commit 7bf923a
Show file tree

Hide file tree

Showing 6 changed files with 57 additions and 20 deletions.
diff --git a/rio/commands/manualRot.py b/rio/commands/manualRot.py
@@ -8,7 +8,7 @@ def __init__(self, speed, _shooter: Shooter):
         super().__init__()
 
         # local subsystem instance
-        self.subsystem = Shooter
+        self.subsystem = _shooter
 
         # requested speed
         self.speed = speed

diff --git a/rio/robot.py b/rio/robot.py
@@ -12,7 +12,7 @@
 from robotcontainer import RobotContainer
 
 
-class MyRobot(commands2.TimedCommandRobot):
+class HolyToaster(commands2.TimedCommandRobot):
     def robotInit(self):
         # Instantiate our RobotContainer.  This will perform all our button bindings, and put our
         # autonomous chooser on the dashboard.
@@ -45,4 +45,4 @@ def testInit(self) -> None:
 
 
 if __name__ == "__main__":
-    wpilib.run(MyRobot)
+    wpilib.run(HolyToaster)
diff --git a/rio/simgui-window.json b/rio/simgui-window.json
@@ -18,7 +18,7 @@
     "###FMS": {
       "Collapsed": "0",
       "Pos": "5,540",
-      "Size": "283,140"
+      "Size": "169,184"
     },
     "###Joysticks": {
       "Collapsed": "0",
@@ -27,13 +27,13 @@
     },
     "###NetworkTables": {
       "Collapsed": "0",
-      "Pos": "250,277",
-      "Size": "750,185"
+      "Pos": "250,163",
+      "Size": "750,299"
     },
     "###NetworkTables Info": {
       "Collapsed": "0",
-      "Pos": "250,130",
-      "Size": "750,145"
+      "Pos": "250,22",
+      "Size": "750,138"
     },
     "###Other Devices": {
       "Collapsed": "0",

diff --git a/rio/simgui.json b/rio/simgui.json
@@ -20,13 +20,17 @@
   },
   "NTProvider": {
     "types": {
-      "/FMSInfo": "FMSInfo"
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/FieldCentric": "String Chooser"
     }
   },
   "NetworkTables": {
     "transitory": {
       "SmartDashboard": {
         "Swerve": {
+          "FL": {
+            "open": true
+          },
           "open": true
         },
         "open": true

diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py
@@ -101,25 +101,51 @@ def periodic(self) -> None:
             ),
         )
 
-        # desired
+        # Desired Positions
         self.sd.putNumber(
-            "Swerve/FL desired", self.frontLeft.desiredState.angle.degrees()
+            "Swerve/FL/desired",
+            self.frontLeft.desiredState.angle.degrees(),
         )
         self.sd.putNumber(
-            "Swerve/FR desired", self.frontRight.desiredState.angle.degrees()
+            "Swerve/FR/desired",
+            self.frontRight.desiredState.angle.degrees(),
         )
         self.sd.putNumber(
-            "Swerve/RL desired", self.rearLeft.desiredState.angle.degrees()
+            "Swerve/RL/desired",
+            self.rearLeft.desiredState.angle.degrees(),
         )
         self.sd.putNumber(
-            "Swerve/RR desired", self.rearRight.desiredState.angle.degrees()
+            "Swerve/RR/desired",
+            self.rearRight.desiredState.angle.degrees(),
         )
 
-        # actual
-        self.sd.putNumber("Swerve/FL", self.frontLeft.getState().angle.degrees())
-        self.sd.putNumber("Swerve/FR", self.frontRight.getState().angle.degrees())
-        self.sd.putNumber("Swerve/RL", self.rearLeft.getState().angle.degrees())
-        self.sd.putNumber("Swerve/RR", self.rearRight.getState().angle.degrees())
+        # Actual Positions
+        self.sd.putNumber(
+            "Swerve/FL/actual",
+            self.frontLeft.getState().angle.degrees(),
+        )
+        self.sd.putNumber(
+            "Swerve/FR/actual",
+            self.frontRight.getState().angle.degrees(),
+        )
+        self.sd.putNumber(
+            "Swerve/RL/actual",
+            self.rearLeft.getState().angle.degrees(),
+        )
+        self.sd.putNumber(
+            "Swerve/RR/actual",
+            self.rearRight.getState().angle.degrees(),
+        )
+
+        # Thermals
+        self.sd.putNumber("Thermals/Swerve/FL/drive", self.frontLeft.getDriveTemp())
+        self.sd.putNumber("Thermals/Swerve/FR/drive", self.frontRight.getDriveTemp())
+        self.sd.putNumber("Thermals/Swerve/RL/drive", self.rearLeft.getDriveTemp())
+        self.sd.putNumber("Thermals/Swerve/RR/drive", self.rearRight.getDriveTemp())
+        self.sd.putNumber("Thermals/Swerve/FL/turn", self.frontLeft.getTurnTemp())
+        self.sd.putNumber("Thermals/Swerve/FR/turn", self.frontRight.getTurnTemp())
+        self.sd.putNumber("Thermals/Swerve/RL/turn", self.rearLeft.getTurnTemp())
+        self.sd.putNumber("Thermals/Swerve/RR/turn", self.rearRight.getTurnTemp())
 
     def getPose(self) -> Pose2d:
         """Returns the currently-estimated pose of the robot.

diff --git a/rio/subsystems/mikeswervemodule.py b/rio/subsystems/mikeswervemodule.py
@@ -166,9 +166,16 @@ def setDesiredState(self, desiredState: SwerveModuleState) -> None:
         )
 
         self.desiredState = desiredState
-
+    
     def resetEncoders(self) -> None:
         """
         Zeroes all the SwerveModule encoders.
         """
         self.drivingEncoder.setPosition(0)
+
+    def getDriveTemp(self) -> float:
+        return self.drivingSparkMax.getMotorTemperature()
+
+    def getTurnTemp(self) -> float:
+        return self.turningSparkMax.getMotorTemperature()
+