wip python logging

PhotonVision · Sep 7, 2024 · d8b6b7c · d8b6b7c
1 parent 63c27a3
commit d8b6b7c
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 19 deletions.
diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py
@@ -25,10 +25,10 @@ def __init__(self) -> None:
         self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381)
         self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381)
 
-        self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3)
-        self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7)
-        self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11)
-        self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15)
+        self.frontLeft  = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3, 1)
+        self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7, 2)
+        self.backLeft   = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11, 3)
+        self.backRight  = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15, 4)
 
         self.gyro = wpilib.AnalogGyro(0)
 
@@ -50,6 +50,8 @@ def __init__(self) -> None:
             ),
         )
 
+        self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds()
+
         self.gyro.reset()
 
     def drive(
@@ -88,6 +90,9 @@ def drive(
         self.backLeft.setDesiredState(swerveModuleStates[2])
         self.backRight.setDesiredState(swerveModuleStates[3])
 
+        self.targetChassisSpeeds = self.kinematics.toChassisSpeeds(swerveModuleStates)
+
+
     def updateOdometry(self) -> None:
         """Updates the field relative position of the robot."""
         self.odometry.update(
@@ -99,3 +104,36 @@ def updateOdometry(self) -> None:
                 self.backRight.getPosition(),
             ),
         )
+
+    def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]:
+        return [ 
+                 self.frontLeft.getState(),
+                 self.frontRight.getState(),
+                 self.backLeft.getState(),
+                 self.backRight.getState(),
+               ]
+
+    def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds:
+        return self.kinematics.toChassisSpeeds(self.getModuleStates())
+
+    def log(self):
+        table = "Drive/"
+
+        pose = self.odometry.getPose()
+        wpilib.SmartDashboard.putNumber(table + "X", pose.X())
+        wpilib.SmartDashboard.putNumber(table + "Y", pose.Y())
+        wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees())
+
+        chassisSpeeds = self.getChassisSpeeds()
+        wpilib.SmartDashboard.putNumber(table + "VX", chassisSpeeds.vx)
+        wpilib.SmartDashboard.putNumber(table + "VY", chassisSpeeds.vy)
+        wpilib.SmartDashboard.putNumber(table + "Omega Degrees", chassisSpeeds.omega_dps)
+
+        wpilib.SmartDashboard.putNumber(table + "Target VX", self.targetChassisSpeeds.vx)
+        wpilib.SmartDashboard.putNumber(table + "Target VY", self.targetChassisSpeeds.vy)
+        wpilib.SmartDashboard.putNumber(table + "Target Omega Degrees", self.targetChassisSpeeds.omega_dps)
+
+        self.frontLeft.log()
+        self.frontRight.log()
+        self.backLeft.log()
+        self.backRight.log()
diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py
@@ -15,7 +15,8 @@ def robotInit(self) -> None:
         self.controller = wpilib.XboxController(0)
         self.swerve = drivetrain.Drivetrain()
 
-
+    def robotPeriodic(self) -> None:
+        self.swerve.log()
 
     def teleopPeriodic(self) -> None:
         xSpeed = -1.0 * self.controller.getLeftY()  * drivetrain.kMaxSpeed

diff --git a/photonlib-python-examples/poseest/swervemodule.py b/photonlib-python-examples/poseest/swervemodule.py
@@ -146,21 +146,12 @@ def setDesiredState(
     def log(self):
         state = self.getState()
 
-        table = "Module " + self.moduleNumber + "/"
+        table = "Module " + str(self.moduleNumber) + "/"
         wpilib.SmartDashboard.putNumber(
-                table + "Steer Degrees", _radToDeg(wpimath.angleModulus(state.angle.getRadians())))
+                table + "Steer Degrees", math.degrees(wpimath.angleModulus(state.angle.radians())))
         wpilib.SmartDashboard.putNumber(
-                table + "Steer Target Degrees", _radToDeg(self.turningPIDController.getSetpoint()))
+                table + "Steer Target Degrees", math.degrees(self.turningPIDController.getSetpoint().position))
         wpilib.SmartDashboard.putNumber(
-                table + "Drive Velocity Feet", _mToft(state.speedMetersPerSecond))
+                table + "Drive Velocity Feet", state.speed_fps)
         wpilib.SmartDashboard.putNumber(
-                table + "Drive Velocity Target Feet",
-                _mToft(self.desiredState.speedMetersPerSecond))
-
-
-
-def _radToDeg(in_val:float)->float:
-    return in_val * 180 / math.pi
-
-def _mToft(in_val:float)->float:
-    return in_val * 3.28084
+                table + "Drive Velocity Target Feet", self.desiredState.speed_fps)