logging reduced, removed long-running background tasks that were caus…

…ing inconsistency
RobotCasserole1736 · Oct 24, 2024 · a066854 · a066854
1 parent a4285b4
commit a066854
Show file tree

Hide file tree

Showing 9 changed files with 48 additions and 39 deletions.
diff --git a/drivetrain/controlStrategies/holonomicDriveController.py b/drivetrain/controlStrategies/holonomicDriveController.py
@@ -43,12 +43,12 @@ def __init__(self, name:str):
         self.yFB = 0.0
         self.tFB = 0.0
 
-        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")
+        #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(

diff --git a/drivetrain/poseEstimation/drivetrainPoseEstimator.py b/drivetrain/poseEstimation/drivetrainPoseEstimator.py
@@ -59,7 +59,7 @@ def __init__(self, initialModulePositions:posTuple_t):
 
         # Logging and Telemetry
         addLog("PE Vision Targets Seen", lambda: self._camTargetsVisible, "bool")
-        addLog("PE Gyro Angle", self._curRawGyroAngle.degrees, "deg")
+        #addLog("PE Gyro Angle", self._curRawGyroAngle.degrees, "deg")
         self._telemetry = DrivetrainPoseTelemetry()
 
         # Simulation Only - maintain a rough estimate of pose from velocities

diff --git a/humanInterface/driverInterface.py b/humanInterface/driverInterface.py
@@ -36,12 +36,12 @@ def __init__(self):
         self.gyroResetCmd = False
 
         # Logging
-        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 autoDriveToSpeaker", lambda: self.autoDriveToSpeaker, "bool")
-        addLog("DI autoDriveToPickup", lambda: self.autoDriveToPickup, "bool")
+        #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 autoDriveToSpeaker", lambda: self.autoDriveToSpeaker, "bool")
+        #addLog("DI autoDriveToPickup", lambda: self.autoDriveToPickup, "bool")
 
     def update(self):
         # value of contoller buttons

diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py
@@ -101,9 +101,9 @@ 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")
+        #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):
         """

diff --git a/robot.py b/robot.py
@@ -22,6 +22,7 @@
 from wpimath.geometry import Translation2d, Pose2d, Rotation2d
 
 class MyRobot(wpilib.TimedRobot):
+
     #########################################################
     ## Common init/update for all modes
     def robotInit(self):

diff --git a/utils/powerMonitor.py b/utils/powerMonitor.py
@@ -60,9 +60,9 @@ class PowerMonitor:
     """
     def __init__(self):
         self.powerDist = wpilib.PowerDistribution()
-        addLog("Battery current draw",self.powerDist.getTotalCurrent, "A")
-        addLog("RIO Voltage",wpilib.RobotController.getBatteryVoltage, "V")
-        addLog("Battery voltage", wpilib.RobotController.getInputVoltage, "V")
+        #addLog("Battery current draw",self.powerDist.getTotalCurrent, "A")
+        #addLog("RIO Voltage",wpilib.RobotController.getBatteryVoltage, "V")
+        #addLog("Battery voltage", wpilib.RobotController.getInputVoltage, "V")
 
     def update(self):
         return
diff --git a/utils/rioMonitor.py b/utils/rioMonitor.py
@@ -23,11 +23,16 @@ def __init__(self):
         self.prevSystemTime = 0
         self.prevIdleTime = 0
 
-        self.thread1 = Thread(target=self._updateFast, daemon=True)
-        self.thread2 = Thread(target=self._updateSlow, daemon=True)
         self.runCmd = True
+
+        self.thread1 = Thread(target=self._updateFast, daemon=True)
         self.thread1.start()
-        self.thread2.start()
+
+        # Note - this is currently taking a VERY long time to run.
+        # We need to redesign this to actually be multithreaded, as the 
+        # GIL is killing us. For now, commented out.
+        #self.thread2 = Thread(target=self._updateSlow, daemon=True)
+        #self.thread2.start()
 
         self.CANBusUsage = 0
         self.CANErrCount = 0
@@ -38,36 +43,39 @@ def __init__(self):
         self.extDiskUsage = 0
 
         addLog("RIO Supply Voltage", RobotController.getInputVoltage, "V")
-        addLog("RIO CAN Bus Usage", lambda: self.CANBusUsage, "pct")
-        addLog(
-            "RIO CAN Bus Err Count",
-             lambda: self.CANErrCount,
-            "count",
-        )
-        addLog("RIO Memory Usage", lambda: self.memUsagePct , "pct")
-        addLog("RIO Internal Disk Usage", lambda: self.intDiskUsage, "pct")
-        addLog(f"RIO USB Disk Usage",lambda: self.extDiskUsage, "pct")
-        addLog("RIO CPU Load",lambda: self.cpuLoad , "pct")
+        #addLog("RIO CAN Bus Usage", lambda: self.CANBusUsage, "pct")
+        #addLog(
+        #    "RIO CAN Bus Err Count",
+        #     lambda: self.CANErrCount,
+        #    "count",
+        #)
+        #addLog("RIO Memory Usage", lambda: self.memUsagePct , "pct")
+        #addLog("RIO Internal Disk Usage", lambda: self.intDiskUsage, "pct")
+        #addLog(f"RIO USB Disk Usage",lambda: self.extDiskUsage, "pct")
+        #addLog("RIO CPU Load",lambda: self.cpuLoad , "pct")
 
 
 
     def stopThreads(self):
         self.runCmd = False
         self.thread1.join()
-        self.thread2.join()
+        #self.thread2.join()
 
     # Things that should be recorded fairly quickly
     def _updateFast(self):
         while self.runCmd:
             self._updateVoltages()
-            time.sleep(0.02)
+            time.sleep(1.0)
 
     # Things that don't have to be updated as fast
     def _updateSlow(self):
         while self.runCmd:
             self._updateMemStats()
+            time.sleep(1.0)
             self._updateCPUStats()
+            time.sleep(1.0)
             self._updateCANStats()
+            time.sleep(1.0)
             self._updateDiskStats()
             time.sleep(1.0)
 

diff --git a/wrappers/wrapperedPulseWidthEncoder.py b/wrappers/wrapperedPulseWidthEncoder.py
@@ -41,8 +41,8 @@ def __init__(
         self.freq = 0
         self.pulseTime = 0
 
-        addLog(f"{self.name}_freq", lambda: self.freq, "Hz")
-        addLog(f"{self.name}_pulseTime", lambda: self.pulseTime, "sec")
+        #addLog(f"{self.name}_freq", lambda: self.freq, "Hz")
+        #addLog(f"{self.name}_pulseTime", lambda: self.pulseTime, "sec")
         addLog(f"{self.name}_angle", lambda: self.curAngleRad, "rad")
 
     def update(self):

diff --git a/wrappers/wrapperedSparkMax.py b/wrappers/wrapperedSparkMax.py
@@ -75,10 +75,10 @@ def __init__(self, canID, name, brakeMode=False, currentLimitA=40.0):
         self.disconFault.set(not self.configSuccess)
 
         addLog(self.name + "_outputCurrent", self.ctrl.getOutputCurrent, "A")
-        addLog(self.name + "_appliedOutput", self.ctrl.getAppliedOutput, "%")
+        #addLog(self.name + "_appliedOutput", self.ctrl.getAppliedOutput, "%")
         addLog(self.name + "_desVolt", lambda: self.desVolt, "V")
-        addLog(self.name + "_desPos", lambda: self.desPos, "rad")
-        addLog(self.name + "_desVel", lambda: self.desVel, "RPM")
+        #addLog(self.name + "_desPos", lambda: self.desPos, "rad")
+        #addLog(self.name + "_desVel", lambda: self.desVel, "RPM")
         addLog(self.name + "_actPos", lambda: self.actPos, "rad")
         addLog(self.name + "_actVel", lambda: self.actVel, "RPM")