Swerve simulation functional

PhotonVision · Sep 8, 2024 · bda58ad · bda58ad
1 parent 137fa36
commit bda58ad
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 26 deletions.
diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py
@@ -156,11 +156,13 @@ def log(self):
         self.backLeft.log()
         self.backRight.log()
 
-        self.debugField.getObject("EstimatedRobot").setPose(self.odometry.getPose())
-        self.debugField.getObject("EstimatedRobotModules").setPoses(self.getModulePoses())
+        self.debugField.getRobotObject().setPose(self.odometry.getPose())
+        self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses())
 
     def simulationPeriodic(self):
         self.frontLeft.simulationPeriodic()
         self.frontRight.simulationPeriodic()
         self.backLeft.simulationPeriodic()
-        self.backRight.simulationPeriodic()
+        self.backRight.simulationPeriodic()
+        self.simGyro.setRate(self.getChassisSpeeds().omega_dps)
+        self.simGyro.setAngle(self.simGyro.getAngle() + self.simGyro.getRate() * 0.02)
diff --git a/photonlib-python-examples/poseest/simgui.json b/photonlib-python-examples/poseest/simgui.json
@@ -9,6 +9,13 @@
       "window": {
         "visible": true
       }
+    },
+    "Other Devices": {
+      "AnalogGyro[0]": {
+        "header": {
+          "open": true
+        }
+      }
     }
   },
   "NTProvider": {
@@ -18,10 +25,10 @@
     },
     "windows": {
       "/SmartDashboard/Drivetrain Debug": {
-        "EstimatedRobotModules": {
-          "image": "D:\\Projects\\photonvision\\photonlib-python-examples\\poseest\\swerve_module.png",
-          "length": 0.4000000059604645,
-          "width": 0.4000000059604645
+        "SwerveModules": {
+          "image": "swerve_module.png",
+          "length": 0.5,
+          "width": 0.5
         },
         "bottom": 1476,
         "height": 8.210550308227539,
@@ -39,16 +46,13 @@
     "transitory": {
       "SmartDashboard": {
         "Drivetrain Debug": {
-          "double[]##v_/SmartDashboard/Drivetrain Debug/EstimatedRobot": {
-            "open": true
-          },
-          "double[]##v_/SmartDashboard/Drivetrain Debug/EstimatedRobotModules": {
-            "open": true
-          },
           "double[]##v_/SmartDashboard/Drivetrain Debug/Robot": {
             "open": true
           }
         },
+        "Module 1": {
+          "open": true
+        },
         "open": true
       }
     }

diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py
@@ -52,22 +52,14 @@ def __init__(
         )
 
         # Gains are for example purposes only - must be determined for your own robot!
-        self.drivePIDController = wpimath.controller.PIDController(1, 0, 0)
+        self.drivePIDController = wpimath.controller.PIDController(10, 0, 0)
 
         # Gains are for example purposes only - must be determined for your own robot!
-        self.turningPIDController = wpimath.controller.ProfiledPIDController(
-            3,
-            0,
-            0,
-            wpimath.trajectory.TrapezoidProfile.Constraints(
-                kModuleMaxAngularVelocity,
-                kModuleMaxAngularAcceleration,
-            ),
-        )
+        self.turningPIDController = wpimath.controller.PIDController(30,0,0)
 
         # Gains are for example purposes only - must be determined for your own robot!
         self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3)
-        self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.5)
+        self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.7)
 
         # Set the distance per pulse for the drive encoder. We can simply use the
         # distance traveled for one rotation of the wheel divided by the encoder
@@ -150,7 +142,7 @@ def setDesiredState(
         )
 
         turnFeedforward = self.turnFeedforward.calculate(
-            self.turningPIDController.getSetpoint().velocity
+            self.turningPIDController.getSetpoint()
         )
 
         self.driveMotor.setVoltage(driveOutput + driveFeedforward)
@@ -166,7 +158,7 @@ def log(self) -> None:
         wpilib.SmartDashboard.putNumber(
                 table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians())))
         wpilib.SmartDashboard.putNumber(
-                table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint().position))
+                table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint()))
         wpilib.SmartDashboard.putNumber(
                 table + "Drive Velocity Feet", state.speed_fps)
         wpilib.SmartDashboard.putNumber(