one million bugfixes and comments

Autonomous/commands/drivePathCommand.py

@@ -34,7 +34,7 @@ def __init__(self, pathFile):
         # we'll populate these for real later, just declare they'll exist
         self.duration = self.path.getTotalTime()
         self.drivetrain = DrivetrainControl()
-        self.poseTelem = self.drivetrain.poseEst.telemetry
+        self.poseTelem = self.drivetrain.poseEst._telemetry
 
     def initialize(self):
         self.startTime = Timer.getFPGATimestamp()

drivetrain/controlStrategies/autoDrive.py

@@ -21,16 +21,13 @@ def __init__(self):
         self._toSpeaker = False
         self._toPickup = False
         self.rfp = RepulsorFieldPlanner()
-        self._trajCtrl = HolonomicDriveController()
+        self._trajCtrl = HolonomicDriveController("AutoDrive")
         self._telemTraj = []
         self._obsDet = ObstacleDetector()
         self._olCmd = DrivetrainCommand()
         self._prevCmd:DrivetrainCommand|None = None
         self._plannerDur:float = 0.0
 
-        addLog("AutoDrive FwdRev Cmd", lambda: self._olCmd.velX, "mps")
-        addLog("AutoDrive Strafe Cmd", lambda: self._olCmd.velY, "mps")
-        addLog("AutoDrive Rot Cmd", lambda: self._olCmd.velT, "radpers")
         addLog("AutoDrive Proc Time", lambda:(self._plannerDur * 1000.0), "ms")
 
 

drivetrain/controlStrategies/holonomicDriveController.py

@@ -16,22 +16,25 @@ class HolonomicDriveController:
     """
     Closed-loop controller suite to get the robot from where it is to where it isn't
     https://www.youtube.com/watch?v=bZe5J8SVCYQ
-    Used to emulate driver commands while following a trajectory.
+    Used to emulate driver commands while following a trajectory or auto-driving.
 
-    This is often called a "Holonomic Drive Controller" or "HDC"
+    This is often called a "Holonomic Drive Controller" or "HDC".
+
+    Note that wpilib has one of these, but it doesn't (yet) include feed-forward on rotation (??????)
+    So we made our own.
     """
 
-    def __init__(self, ):
+    def __init__(self, name:str):
         self.curVx = 0
         self.curVy = 0
         self.curVtheta = 0
 
-        self.transP = Calibration(f"Drivetrain HDC Translation kP", 6.0)
-        self.transI = Calibration(f"Drivetrain HDC Translation kI", 0.0)
-        self.transD = Calibration(f"Drivetrain HDC Translation kD", 0.0)
-        self.rotP = Calibration(f"Drivetrain HDC Rotation kP", 2.0)
-        self.rotI = Calibration(f"Drivetrain HDC Rotation kI", 0.0)
-        self.rotD = Calibration(f"Drivetrain HDC Rotation kD", .05)
+        self.transP = Calibration(f"{name} HDC Translation kP", 6.0)
+        self.transI = Calibration(f"{name} HDC Translation kI", 0.0)
+        self.transD = Calibration(f"{name} HDC Translation kD", 0.0)
+        self.rotP = Calibration(f"{name} HDC Rotation kP", 2.0)
+        self.rotI = Calibration(f"{name} HDC Rotation kI", 0.0)
+        self.rotD = Calibration(f"{name} HDC Rotation kD", .05)
 
         self.xFF = 0.0
         self.yFF = 0.0
@@ -40,12 +43,12 @@ def __init__(self, ):
         self.yFB = 0.0
         self.tFB = 0.0
 
-        addLog("Drivetrain HDC xFF", lambda:self.xFF, "mps")
-        addLog("Drivetrain HDC yFF", lambda:self.yFF, "mps")
-        addLog("Drivetrain HDC tFF", lambda:self.tFF, "radpersec")
-        addLog("Drivetrain HDC xFB", lambda:self.xFB, "mps")
-        addLog("Drivetrain HDC yFB", lambda:self.yFB, "mps")
-        addLog("Drivetrain HDC tFB", lambda:self.tFB, "radpersec")
+        addLog(f"{name} HDC xFF", lambda:self.xFF, "mps")
+        addLog(f"{name} HDC yFF", lambda:self.yFF, "mps")
+        addLog(f"{name} HDC tFF", lambda:self.tFF, "radpersec")
+        addLog(f"{name} HDC xFB", lambda:self.xFB, "mps")
+        addLog(f"{name} HDC yFB", lambda:self.yFB, "mps")
+        addLog(f"{name} HDC tFB", lambda:self.tFB, "radpersec")
 
         # Closed-loop control for the X position
         self.xCtrl = PIDController(

drivetrain/controlStrategies/trajectory.py

@@ -9,7 +9,7 @@
 
 class Trajectory(metaclass=Singleton):
     def __init__(self):
-        self.trajCtrl = HolonomicDriveController()
+        self.trajHDC = HolonomicDriveController("Trajectory")
         self.curTrajCmd = None
 
     def setCmd(self, cmd: ChoreoTrajectoryState | None):
@@ -22,6 +22,6 @@ def setCmd(self, cmd: ChoreoTrajectoryState | None):
 
     def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand:
         if self.curTrajCmd is not None:
-            return self.trajCtrl.update(self.curTrajCmd, curPose)
+            return self.trajHDC.update(self.curTrajCmd, curPose)
         else:
             return cmdIn

drivetrain/drivetrainCommand.py

@@ -2,10 +2,13 @@
 from dataclasses import dataclass
 
 
-# Represents desired drivetrain motion which is currently desired.
-# Usually comes from a human driver, but could be from an autonomous momde or assist feature.
+
 @dataclass
 class DrivetrainCommand:
+    """
+    Represents desired drivetrain motion which is currently desired.
+    Usually comes from a human driver, but could be from an autonomous momde or assist feature.
+    """
     velX:float = 0.0  # Field X velocity in meters/sec
     velY:float = 0.0  # Field Y velocity in meters/sec
     velT:float = 0.0  # Rotational speed in rad/sec

drivetrain/drivetrainControl.py

@@ -97,7 +97,7 @@ def update(self):
             desPose = self.curCmd.desPose
         else:
             desPose = curEstPose
-        self.poseEst.telemetry.setDesiredPose(desPose)
+        self.poseEst._telemetry.setDesiredPose(desPose)
 
         # Given the current desired chassis speeds, convert to module states
         desModStates = kinematics.toSwerveModuleStates(self.desChSpd)

drivetrain/poseEstimation/drivetrainPoseEstimator.py

@@ -13,38 +13,61 @@
 from utils.faults import Fault
 from utils.signalLogging import addLog
 from wrappers.wrapperedPoseEstPhotonCamera import WrapperedPoseEstPhotonCamera
-
+from wpimath.kinematics import SwerveModulePosition, SwerveModuleState
+
+# Convienent abreviations for the types that we'll be passing around here.
+# This is primarily driven by wpilib's conventions:
+# 1) Swerve objects will be put into a tuple, with the length of the tuple equal to the number of modules
+# 2) "State" refers to the speed of the wheel, plus the position of the azimuth
+# 3) "Position" refers to the position of the wheel, plus the position of the azimuth
+# Wheel position is preferred for odometry, since errors in velocity don't accumulate over time
+# This is especially important with Rev motors which filter their velocity by a huge amount
+# but the position is fairly accurate. 
+posTuple_t = tuple[SwerveModulePosition,SwerveModulePosition,SwerveModulePosition,SwerveModulePosition]
+stateTuple_t = tuple[SwerveModuleState,SwerveModuleState,SwerveModuleState,SwerveModuleState]
 
 class DrivetrainPoseEstimator:
     """Wrapper class for all sensors and logic responsible for estimating where the robot is on the field"""
 
-    def __init__(self, initialModuleStates):
-        self.curEstPose = Pose2d()
-        self.curDesPose = Pose2d()
-        self.gyro = ADIS16470_IMU()
-        self.gyroDisconFault = Fault("Gyro Disconnected")
+    def __init__(self, initialModulePositions:posTuple_t):
+
+        # Represents our current best-guess as to our location on the field.
+        self._curEstPose = Pose2d()
 
+        # Gyroscope - measures our rotational velocity.
+        # Fairly accurate and trustworthy, but not a full pose estimate
+        self._gyro = ADIS16470_IMU()
+        self._gyroDisconFault = Fault("Gyroscope not sending data")
+        self._curRawGyroAngle = Rotation2d()
+
+        # Cameras - measure our position on the field from apriltags
+        # Generally accurate, but slow and laggy. Might need to be disabled
+        # if the robot isn't flat on the ground for some reason.
         self.cams = [
             WrapperedPoseEstPhotonCamera("LEFT_CAM", ROBOT_TO_LEFT_CAM),
             WrapperedPoseEstPhotonCamera("RIGHT_CAM", ROBOT_TO_RIGHT_CAM),
         ]
-        self.camTargetsVisible = False
+        self._camTargetsVisible = False
+        self._useAprilTags = True
 
-        self.poseEst = SwerveDrive4PoseEstimator(
-            kinematics, self._getGyroAngle(), initialModuleStates, self.curEstPose
+        # The kalman filter to fuse all sources of information together into a single
+        # best-estimate of the pose of the field
+        self._poseEst = SwerveDrive4PoseEstimator(
+            kinematics, self._getGyroAngle(), initialModulePositions, self._curEstPose
         )
-        self.lastModulePositions = initialModuleStates
-        self.curRawGyroAngle = Rotation2d()
-        self.telemetry = DrivetrainPoseTelemetry()
-
-        self._simPose = Pose2d()
+        self._lastModulePositions = initialModulePositions
 
-        self.useAprilTags = True
+        # Logging and Telemetry
+        addLog("PE Vision Targets Seen", lambda: self._camTargetsVisible, "bool")
+        addLog("PE Gyro Angle", self._curRawGyroAngle.degrees, "deg")
+        self._telemetry = DrivetrainPoseTelemetry()
 
-        addLog("PE Vision Targets Seen", lambda: self.camTargetsVisible, "bool")
-        addLog("PE Gyro Angle", self.curRawGyroAngle.degrees, "deg")
+        # Simulation Only - maintain a rough estimate of pose from velocities
+        # Using just inverse kinematics, no kalman filter. This is used only
+        # to produce a reasonable-looking simulated gyroscope.
+        self._simPose = Pose2d()
 
-    def setKnownPose(self, knownPose):
+    def setKnownPose(self, knownPose:Pose2d):
         """Reset the robot's estimated pose to some specific position. This is useful if we know with certanty
         we are at some specific spot (Ex: start of autonomous)
 
@@ -53,69 +76,76 @@ def setKnownPose(self, knownPose):
         """
         if wpilib.TimedRobot.isSimulation():
             self._simPose = knownPose
-            self.curRawGyroAngle = knownPose.rotation()
+            self._curRawGyroAngle = knownPose.rotation()
 
-        self.poseEst.resetPosition(
-            self.curRawGyroAngle, self.lastModulePositions, knownPose
+        self._poseEst.resetPosition(
+            self._curRawGyroAngle, self._lastModulePositions, knownPose
         )
 
-    def update(self, curModulePositions, curModuleSpeeds):
+    def update(self, curModulePositions:posTuple_t, curModuleSpeeds:stateTuple_t):
         """Periodic update, call this every 20ms.
 
         Args:
-            curModulePositions (list[SwerveModuleState]): current module angle
+            curModulePositions  current module angle
             and wheel positions as read in from swerve module sensors
         """
 
         # Add any vision observations to the pose estimate
-        self.camTargetsVisible = False
+        self._camTargetsVisible = False
 
-        if(self.useAprilTags):
+        if(self._useAprilTags):
             for cam in self.cams:
-                cam.update(self.curEstPose)
+                cam.update(self._curEstPose)
                 observations = cam.getPoseEstimates()
                 for observation in observations:
-                    self.poseEst.addVisionMeasurement(
+                    self._poseEst.addVisionMeasurement(
                         observation.estFieldPose, observation.time
                     )
-                    self.camTargetsVisible = True
-                self.telemetry.addVisionObservations(observations)
+                    self._camTargetsVisible = True
+                self._telemetry.addVisionObservations(observations)
 
 
         # Read the gyro angle
-        self.gyroDisconFault.set(not self.gyro.isConnected())
+        self._gyroDisconFault.set(not self._gyro.isConnected())
         if wpilib.TimedRobot.isSimulation():
+            # Simulated Gyro
             # Simulate an angle based on (simulated) motor speeds with some noise
             chSpds = kinematics.toChassisSpeeds(curModuleSpeeds)
             self._simPose = self._simPose.exp(
                 Twist2d(chSpds.vx * 0.02, chSpds.vy * 0.02, chSpds.omega * 0.02)
             )
             noise = Rotation2d.fromDegrees(random.uniform(-0.0, 0.0))
-            self.curRawGyroAngle = self._simPose.rotation() + noise
+            self._curRawGyroAngle = self._simPose.rotation() + noise
         else:
-            # Use real hardware
-            self.curRawGyroAngle = self._getGyroAngle()
+            # Real Gyro
+            # Read the value from the hardware
+            self._curRawGyroAngle = self._getGyroAngle()
 
         # Update the WPILib Pose Estimate
-        self.poseEst.update(self.curRawGyroAngle, curModulePositions)
-        self.curEstPose = self.poseEst.getEstimatedPosition()
+        self._poseEst.update(self._curRawGyroAngle, curModulePositions)
+        self._curEstPose = self._poseEst.getEstimatedPosition()
 
         # Record the estimate to telemetry/logging-
-        self.telemetry.update(self.curEstPose, [x.angle for x in curModulePositions])
+        self._telemetry.update(self._curEstPose, [x.angle for x in curModulePositions])
 
         # Remember the module positions for next loop
-        self.lastModulePositions = curModulePositions
+        self._lastModulePositions = curModulePositions
 
-    def getCurEstPose(self):
+    def getCurEstPose(self)->Pose2d:
         """
         Returns:
             Pose2d: The most recent estimate of where the robot is at
         """
-        return self.curEstPose
+        return self._curEstPose
 
-    def setUseAprilTags(self, use):
-        self.useAprilTags = use
+    def setUseAprilTags(self, use:bool):
+        """
+        Enables or disables pose estimate correction based on apriltag readings.
+        Useful if the robot is known to be doing something (like tilting) where
+        the pose estimate will go inaccurate
+        """
+        self._useAprilTags = use
 
     # Local helper to wrap the real hardware angle into a Rotation2d
-    def _getGyroAngle(self):
-        return Rotation2d().fromDegrees(self.gyro.getAngle(self.gyro.getYawAxis()))
+    def _getGyroAngle(self)->Rotation2d:
+        return Rotation2d().fromDegrees(self._gyro.getAngle(self._gyro.getYawAxis()))