performed log optimization

dashboard.py

@@ -5,7 +5,7 @@
 from dashboardWidgets.icon import Icon
 from dashboardWidgets.text import Text
 from utils.faults import FaultWrangler
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 from webserver.webserver import Webserver
 
 
@@ -35,12 +35,21 @@ def __init__(self):
             )
         )
 
-    def update(self):
-        log("isRedIconState",  
+        # Add logging for things that don't come from anywhere else
+        addLog("isRedIconState",  
+               lambda: (
             Icon.kON if wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed 
             else Icon.kOFF)
-        log("isBlueIconState", 
+        )
+
+        addLog("isBlueIconState", 
+            lambda: (
             Icon.kON if wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kBlue 
             else Icon.kOFF)
-        log("faultIconState", Icon.kBLINK_FAST if FaultWrangler().hasActiveFaults() else Icon.kOFF)
+        )
+
+        addLog("faultIconState",
+                lambda: (Icon.kBLINK_FAST if FaultWrangler().hasActiveFaults() else Icon.kOFF)
+        )
+
 

drivetrain/controlStrategies/autoDrive.py

@@ -4,7 +4,7 @@
 from drivetrain.controlStrategies.holonomicDriveController import HolonomicDriveController
 from drivetrain.drivetrainCommand import DrivetrainCommand
 from navigation.obstacleDetector import ObstacleDetector
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 from utils.singleton import Singleton
 from navigation.repulsorFieldPlanner import RepulsorFieldPlanner
 from navigation.navConstants import GOAL_PICKUP, GOAL_SPEAKER
@@ -24,9 +24,16 @@ def __init__(self):
         self._trajCtrl = HolonomicDriveController()
         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")
+
+
     def setRequest(self, toSpeaker, toPickup) -> None:
         self._toSpeaker = toSpeaker
         self._toPickup = toPickup
@@ -35,7 +42,7 @@ def getTrajectory(self) -> Trajectory|None:
         return None # TODO
 
     def updateTelemetry(self) -> None:        
-        self._telemTraj = self.rfp.updateTelemetry()
+        self._telemTraj = self.rfp.getLookaheadTraj()
 
     def getWaypoints(self) -> list[Pose2d]:
         return self._telemTraj
@@ -68,23 +75,24 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
         # If being asked to auto-align, use the command from the dynamic path planner
         if(self._toPickup or self._toSpeaker):
 
+            # Open Loop - Calculate the new desired pose and velocity to get there from the 
+            # repulsor field path planner
             if(self._prevCmd is None or self._prevCmd.desPose is None):
-                olCmd = self.rfp.update(curPose, MAX_PATHPLAN_SPEED_MPS*0.02)
+                self._olCmd = self.rfp.update(curPose, MAX_PATHPLAN_SPEED_MPS*0.02)
             else:
-                olCmd = self.rfp.update(self._prevCmd.desPose, MAX_PATHPLAN_SPEED_MPS*0.02)
-
-            log("AutoDrive FwdRev Cmd", olCmd.velX, "mps")
-            log("AutoDrive Strafe Cmd", olCmd.velY, "mps")
-            log("AutoDrive Rot Cmd", olCmd.velT, "radpers")
-
-            if( olCmd.desPose is not None):
-                retCmd = self._trajCtrl.update2(olCmd.velX, olCmd.velY, olCmd.velT, olCmd.desPose, curPose)
-
+                self._olCmd = self.rfp.update(self._prevCmd.desPose, MAX_PATHPLAN_SPEED_MPS*0.02)
+
+            # Add closed loop - use the trajectory controller to add in additional 
+            # velocity if we're currently far away from the desired pose
+            if( self._olCmd.desPose is not None):
+                retCmd = self._trajCtrl.update2(self._olCmd.velX, 
+                                                self._olCmd.velY, 
+                                                self._olCmd.velT, 
+                                                self._olCmd.desPose, curPose)    
             self._prevCmd = retCmd
         else:
             self._prevCmd = None
 
         self._plannerDur = Timer.getFPGATimestamp() - startTime
-        log("AutoDrive Proc Time", self._plannerDur * 1000.0, "ms")
 
         return retCmd

drivetrain/controlStrategies/holonomicDriveController.py

@@ -9,7 +9,7 @@
 #from drivetrain.controlStrategies.autoDrive import AutoDrive
 from jormungandr.choreoTrajectory import ChoreoTrajectoryState
 from utils.calibration import Calibration
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 from utils.mathUtils import limit
 
 class HolonomicDriveController:
@@ -21,17 +21,31 @@ class HolonomicDriveController:
     This is often called a "Holonomic Drive Controller" or "HDC"
     """
 
-    def __init__(self):
+    def __init__(self, ):
         self.curVx = 0
         self.curVy = 0
         self.curVtheta = 0
 
-        self.transP = Calibration("Drivetrain HDC Translation kP", 6.0)
-        self.transI = Calibration("Drivetrain HDC Translation kI", 0.0)
-        self.transD = Calibration("Drivetrain HDC Translation kD", 0.0)
-        self.rotP = Calibration("Drivetrain HDC Rotation kP", 2.0)
-        self.rotI = Calibration("Drivetrain HDC Rotation kI", 0.0)
-        self.rotD = Calibration("Drivetrain HDC Rotation kD", .05)
+        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.xFF = 0.0
+        self.yFF = 0.0
+        self.tFF = 0.0
+        self.xFB = 0.0
+        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")
 
         # Closed-loop control for the X position
         self.xCtrl = PIDController(
@@ -82,24 +96,21 @@ def update(self, trajCmd: ChoreoTrajectoryState, curEstPose):
     def update2(self, xFF, yFF, tFF, cmdPose:Pose2d, curEstPose:Pose2d):
         # Feed-Back - Apply additional correction if we're not quite yet at the spot on the field we
         #             want to be at.
-        xFB = self.xCtrl.calculate(curEstPose.X(), cmdPose.X())
-        yFB = self.yCtrl.calculate(curEstPose.Y(), cmdPose.Y())
-        tFB = self.tCtrl.calculate(
+        self.xFB = self.xCtrl.calculate(curEstPose.X(), cmdPose.X())
+        self.yFB = self.yCtrl.calculate(curEstPose.Y(), cmdPose.Y())
+        self.tFB = self.tCtrl.calculate(
             curEstPose.rotation().radians(), cmdPose.rotation().radians()
         )
 
-        log("Drivetrain HDC xFF", xFF, "mps")
-        log("Drivetrain HDC yFF", yFF, "mps")
-        log("Drivetrain HDC tFF", tFF, "radpersec")
-
-        log("Drivetrain HDC xFB", xFB, "mps")
-        log("Drivetrain HDC yFB", yFB, "mps")
-        log("Drivetrain HDC tFB", tFB, "radpersec")
+        # Remember feed-forward value inputs
+        self.xFF = xFF 
+        self.yFF = yFF 
+        self.tFF = tFF 
 
         retVal = DrivetrainCommand()
-        retVal.velX = limit(xFF + xFB, MAX_FWD_REV_SPEED_MPS)
-        retVal.velY = limit(yFF + yFB, MAX_FWD_REV_SPEED_MPS)
-        retVal.velT = limit(tFF + tFB, MAX_ROTATE_SPEED_RAD_PER_SEC)
+        retVal.velX = limit(xFF + self.xFB, MAX_FWD_REV_SPEED_MPS)
+        retVal.velY = limit(yFF + self.yFB, MAX_FWD_REV_SPEED_MPS)
+        retVal.velT = limit(tFF + self.tFB, MAX_ROTATE_SPEED_RAD_PER_SEC)
         retVal.desPose = cmdPose
 
         return retVal

drivetrain/poseEstimation/drivetrainPoseEstimator.py

@@ -11,7 +11,7 @@
 )
 from drivetrain.poseEstimation.drivetrainPoseTelemetry import DrivetrainPoseTelemetry
 from utils.faults import Fault
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 from wrappers.wrapperedPoseEstPhotonCamera import WrapperedPoseEstPhotonCamera
 
 
@@ -41,6 +41,9 @@ def __init__(self, initialModuleStates):
 
         self.useAprilTags = True
 
+        addLog("PE Vision Targets Seen", lambda: self.camTargetsVisible, "bool")
+        addLog("PE Gyro Angle", self.curRawGyroAngle.degrees, "deg")
+
     def setKnownPose(self, knownPose):
         """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)
@@ -78,7 +81,6 @@ def update(self, curModulePositions, curModuleSpeeds):
                     self.camTargetsVisible = True
                 self.telemetry.addVisionObservations(observations)
 
-        log("PE Vision Targets Seen", self.camTargetsVisible, "bool")
 
         # Read the gyro angle
         self.gyroDisconFault.set(not self.gyro.isConnected())
@@ -99,7 +101,6 @@ def update(self, curModulePositions, curModuleSpeeds):
         self.curEstPose = self.poseEst.getEstimatedPosition()
 
         # Record the estimate to telemetry/logging-
-        log("PE Gyro Angle", self.curRawGyroAngle.degrees(), "deg")
         self.telemetry.update(self.curEstPose, [x.angle for x in curModulePositions])
 
         # Remember the module positions for next loop

drivetrain/poseEstimation/drivetrainPoseTelemetry.py

@@ -6,7 +6,7 @@
 from wpimath.geometry import Pose2d, Pose3d, Transform2d, Rotation2d, Translation2d
 from ntcore import NetworkTableInstance
 
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 from utils.allianceTransformUtils import transform
 from drivetrain.drivetrainPhysical import ROBOT_TO_FRONT_CAM, ROBOT_TO_LEFT_CAM, ROBOT_TO_RIGHT_CAM, robotToModuleTranslations
 from wrappers.wrapperedPoseEstPhotonCamera import CameraPoseObservation

drivetrain/swerveModuleControl.py

@@ -13,7 +13,7 @@
 from wrappers.wrapperedSRXMagEncoder import WrapperedSRXMagEncoder
 from dashboardWidgets.swerveState import getAzmthDesTopicName, getAzmthActTopicName
 from dashboardWidgets.swerveState import getSpeedDesTopicName, getSpeedActTopicName
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 from utils.units import rad2Deg
 from utils.faults import Fault
 from utils.robotIdentification import RobotIdentification
@@ -74,42 +74,31 @@ def __init__(
 
         self._prevMotorDesSpeed = 0
 
-        self.moduleName = moduleName
 
-        self.serialFault = Fault(f"Serial Number Unknown")
-
-        # Simulation Support Only
-        self.wheelSimFilter = SlewRateLimiter(24.0)
-
-    def _updateTelemetry(self):
-        """
-        Helper function to put all relevant data to logs and dashboards for this module
-        """
-        log(
-            getAzmthDesTopicName(self.moduleName),
-            self.optimizedDesiredState.angle.degrees(),
+        addLog(
+            getAzmthDesTopicName(moduleName),
+            self.optimizedDesiredState.angle.degrees,
             "deg",
         )
-        log(
-            getAzmthActTopicName(self.moduleName),
-            self.actualState.angle.degrees(),
+        addLog(
+            getAzmthActTopicName(moduleName),
+            self.actualState.angle.degrees,
             "deg",
         )
-        log(
-            getSpeedDesTopicName(self.moduleName),
-            self.optimizedDesiredState.speed / MAX_FWD_REV_SPEED_MPS,
+        addLog(
+            getSpeedDesTopicName(moduleName),
+            lambda: (self.optimizedDesiredState.speed / MAX_FWD_REV_SPEED_MPS),
             "frac",
         )
-        log(
-            getSpeedActTopicName(self.moduleName),
-            (self.actualState.speed) / MAX_FWD_REV_SPEED_MPS,
+        addLog(
+            getSpeedActTopicName(moduleName),
+            lambda: ((self.actualState.speed) / MAX_FWD_REV_SPEED_MPS),
             "frac",
         )
 
-        if RobotIdentification().getSerialFaulted():
-            self.serialFault.setFaulted()
-        else:
-            self.serialFault.setNoFault()
+
+        # Simulation Support Only
+        self.wheelSimFilter = SlewRateLimiter(24.0)
 
     def getActualPosition(self):
         """
@@ -206,6 +195,3 @@ def update(self):
             speed = self.wheelSimFilter.calculate(self.optimizedDesiredState.speed)
             self.actualState.speed = speed + random.uniform(-0.0, 0.0)
             self.actualPosition.distance += self.actualState.speed * 0.02
-
-
-        self._updateTelemetry()

humanInterface/driverInterface.py

@@ -3,7 +3,7 @@
 MAX_ROTATE_SPEED_RAD_PER_SEC,MAX_TRANSLATE_ACCEL_MPS2,MAX_ROTATE_ACCEL_RAD_PER_SEC_2
 from utils.allianceTransformUtils import onRed
 from utils.faults import Fault
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 from wpimath import applyDeadband
 from wpimath.filter import SlewRateLimiter
 from wpilib import XboxController
@@ -27,6 +27,13 @@ def __init__(self):
         self.navToPickup = False
         self.createObstacle = False
 
+        addLog("DI FwdRev Cmd", lambda: self.velXCmd, "mps")
+        addLog("DI Strafe Cmd", lambda: self.velYCmd, "mps")
+        addLog("DI Rot Cmd", lambda: self.velTCmd, "radps")
+        addLog("DI gyroResetCmd", lambda: self.gyroResetCmd, "bool")
+        addLog("DI navToSpeaker", lambda: self.navToSpeaker, "bool")
+        addLog("DI navToPickup", lambda: self.navToPickup, "bool")
+
     def update(self):
         # value of contoller buttons
 
@@ -79,13 +86,7 @@ def update(self):
             self.createObstacle = False
 
 
-        #log("DI FwdRev Cmd", self.velXCmd, "mps")
-        #log("DI Strafe Cmd", self.velYCmd, "mps")
-        #log("DI Rot Cmd", self.velTCmd, "radps")
-        #log("DI connective fault", self.ctrl.isConnected(), "bool")
-        #log("DI gyroResetCmd", self.gyroResetCmd, "bool")
-        #log("DI navToSpeaker", self.navToSpeaker, "bool")
-        #log("DI navToPickup", self.navToPickup, "bool")
+
 
     def getCmd(self):
         retval = DrivetrainCommand()

navigation/repulsorFieldPlanner.py

@@ -1,7 +1,7 @@
 from wpimath.geometry import Pose2d, Translation2d, Rotation2d
 
 from utils.mapLookup2d import MapLookup2D
-from utils.signalLogging import log
+from utils.signalLogging import addLog
 
 from drivetrain.drivetrainCommand import DrivetrainCommand
 
@@ -94,6 +94,10 @@ def __init__(self):
         self.goal:Pose2d|None = None
         self.lookaheadTraj:list[Pose2d] = []
 
+        addLog("PotentialField Num Obstacles", lambda: (len(self.fixedObstacles) + len(self.transientObstcales)))
+        addLog("PotentialField Path Active", lambda: (self.goal is not None))
+        addLog("PotentialField DistToGo", lambda: self.distToGo, "m")
+
     def setGoal(self, goal:Pose2d|None):
         """
         Sets the current goal pose of the planner. This can be changed at any time.
@@ -287,15 +291,11 @@ def _doLookahead(self, curPose):
                     # Weird, no pose given back, give up.
                     break
 
-    def updateTelemetry(self) -> list[Pose2d]:
+    def getLookaheadTraj(self) -> list[Pose2d]:
         """
-        Perform all telemetry-related tasks.
-        This includes logging data
-        Returns the list of Pose2D's that are the result of the lookahead operation
+        gets the current list of lookahead poses
         """
-        #log("PotentialField Num Obstacles", len(self.fixedObstacles) + len(self.transientObstcales))
-        #log("PotentialField Path Active", self.goal is not None)
-        #log("PotentialField DistToGo", self.distToGo, "m")
+
         if(self.goal is not None):
             return self.lookaheadTraj
         else: