diff --git a/dashboard.py b/dashboard.py index 346cf5a..f7beb06 100644 --- a/dashboard.py +++ b/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) + ) + diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index aeb9574..926b436 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/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 \ No newline at end of file diff --git a/drivetrain/controlStrategies/holonomicDriveController.py b/drivetrain/controlStrategies/holonomicDriveController.py index f644a05..ff53fc1 100644 --- a/drivetrain/controlStrategies/holonomicDriveController.py +++ b/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 diff --git a/drivetrain/poseEstimation/drivetrainPoseEstimator.py b/drivetrain/poseEstimation/drivetrainPoseEstimator.py index e366a1f..eafe882 100644 --- a/drivetrain/poseEstimation/drivetrainPoseEstimator.py +++ b/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 diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py index a35e359..560490b 100644 --- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py +++ b/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 diff --git a/drivetrain/swerveModuleControl.py b/drivetrain/swerveModuleControl.py index a8109d4..14a7023 100644 --- a/drivetrain/swerveModuleControl.py +++ b/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() diff --git a/humanInterface/driverInterface.py b/humanInterface/driverInterface.py index 76806ff..d0a6807 100644 --- a/humanInterface/driverInterface.py +++ b/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() diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index cc680de..2a02c8a 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/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: diff --git a/robot.py b/robot.py index 35280d6..e82f440 100644 --- a/robot.py +++ b/robot.py @@ -13,7 +13,7 @@ from humanInterface.ledControl import LEDControl from navigation.forceGenerators import PointObstacle from utils.segmentTimeTracker import SegmentTimeTracker -from utils.signalLogging import log, addLog, update +from utils.signalLogging import addLog, addLog, update import utils.signalLogging from utils.calibration import CalibrationWrangler from utils.faults import FaultWrangler @@ -56,7 +56,6 @@ def robotInit(self): # Normal robot code updates every 20ms, but not everything needs to be that fast. # Register slower-update periodic functions - self.addPeriodic(self.dashboard.update, 0.2, 0.0) self.addPeriodic(self.pwrMon.update, 0.2, 0.0) self.addPeriodic(self.crashLogger.update, 1.0, 0.0) self.addPeriodic(CalibrationWrangler().update, 0.5, 0.0) @@ -64,31 +63,28 @@ def robotInit(self): self.autoHasRun = False - def _testLoggingInit(self): - for i in range(0,100): - addLog(f"testSig{i}", lambda: self.autoHasRun) - - def _testLoggingMany(self): - for _ in range (0,1000): - update() - def robotPeriodic(self): self.stt.start() self.dInt.update() + self.stt.mark("Driver Interface") self.driveTrain.update() + self.stt.mark("Drivetrain") self.autodrive.updateTelemetry() self.driveTrain.poseEst.telemetry.setWPITrajectory(self.autodrive.getTrajectory()) self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints()) self.driveTrain.poseEst.telemetry.setCurObstacles(self.autodrive.getObstacles()) + self.stt.mark("Telemetry") self.ledCtrl.setAutoDrive(self.autodrive.isRunning()) self.ledCtrl.setStuck(self.autodrive.rfp.isStuck()) self.ledCtrl.update() + self.stt.mark("LED Ctrl") + update() self.stt.end() ######################################################### @@ -104,18 +100,6 @@ def autonomousInit(self): # Mark we at least started autonomous self.autoHasRun = True - # Test only - - self._testLoggingInit() - profiler = LineProfiler() - profiler.add_module(utils.signalLogging) - start = wpilib.Timer.getFPGATimestamp() - profiler.runcall(self._testLoggingMany) - end = wpilib.Timer.getFPGATimestamp() - profiler.print_stats() - print(f"TotalTime: {end-start} sec") - - def autonomousPeriodic(self): self.autoSequencer.update() @@ -142,6 +126,8 @@ def teleopInit(self): def teleopPeriodic(self): + # TODO - this is technically one loop delayed, which could induce lag + # Probably not noticeable, but should be corrected. self.driveTrain.setManualCmd(self.dInt.getCmd()) if self.dInt.getGyroResetCmd(): diff --git a/utils/powerMonitor.py b/utils/powerMonitor.py index 83509fb..5718f04 100644 --- a/utils/powerMonitor.py +++ b/utils/powerMonitor.py @@ -1,8 +1,6 @@ #import sys import wpilib -from utils.signalLogging import log - -#class wpilib.RobotController +from utils.signalLogging import addLog ''' @@ -60,9 +58,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") + def update(self): - #log("Battery current draw",self.powerDist.getTotalCurrent(), "A") - #log("RIO Voltage",wpilib.RobotController.getBatteryVoltage(), "V") - #log("Battery voltage", wpilib.RobotController.getInputVoltage(), "V") return diff --git a/utils/rioMonitor.py b/utils/rioMonitor.py index 2c56b3e..edcc4a1 100644 --- a/utils/rioMonitor.py +++ b/utils/rioMonitor.py @@ -5,7 +5,7 @@ from wpilib import RobotController from wpilib import RobotBase from utils.faults import Fault -from utils.signalLogging import log +from utils.signalLogging import addLog # Records faults and runtime metrics for the roboRIO @@ -27,6 +27,28 @@ def __init__(self): self.thread1.start() self.thread2.start() + self.CANBusUsage = 0 + self.CANErrCount = 0 + self.memUsagePct = 0 + self.cpuLoad = 0 + + self.intDiskUsage = 0 + 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") + + + def stopThreads(self): self.runCmd = False self.thread1.join() @@ -66,22 +88,18 @@ def _updateDiskStats(self): pctUsed = usedBytes / float(usedBytes + availBytes) * 100.0 if mountDir == "/": - log("RIO SD Card Disk Usage", pctUsed, "pct") + self.intDiskUsage = pctUsed elif mountDir.startswith("/media"): - mountDir = mountDir.replace("/", "\\") - log(f"RIO USB {mountDir} Disk Usage", pctUsed, "pct") + self.extDiskUsage = pctUsed def _updateCANStats(self): status = RobotController.getCANStatus() - #log("RIO CAN Bus Usage", status.percentBusUtilization, "pct") - #log( - # "RIO CAN Bus Err Count", - # status.txFullCount + status.receiveErrorCount + status.transmitErrorCount, - # "count", - #) + self.CANBusUsage = status.percentBusUtilization + self.CANErrCount = status.txFullCount + status.receiveErrorCount + status.transmitErrorCount + + def _updateVoltages(self): - #log("RIO Supply Voltage", RobotController.getInputVoltage(), "V") if not RobotController.isBrownedOut(): self.railFault3p3v.set(not RobotController.getEnabled3V3()) self.railFault5v.set(not RobotController.getEnabled5V()) @@ -129,7 +147,7 @@ def _updateCPUStats(self): # Calculate and log the Load Percent as percentage of # total time that we were not idle - log("RIO CPU Load", totalInUseTime / totalTime * 100.0, "pct") + self.cpuLoad = totalInUseTime / totalTime * 100.0 # Remember current stats for next time self.prevUserTime = curUserTime @@ -138,6 +156,7 @@ def _updateCPUStats(self): self.prevIdleTime = curIdleTime def _updateMemStats(self): + self.memUsagePct = -1 if RobotBase.isReal(): memTotalStr = None memFreeStr = None @@ -160,5 +179,7 @@ def _updateMemStats(self): curFreeMem = float(memFreeParts[1]) except ValueError: return # Skip this time if we couldn't parse out values + + self.memUsagePct = (1.0 - curFreeMem / curTotalMem) * 100.0 + - log("RIO Memory Usage", (1.0 - curFreeMem / curTotalMem) * 100.0, "pct") diff --git a/utils/robotIdentification.py b/utils/robotIdentification.py index 45cdcdd..e4c44b1 100644 --- a/utils/robotIdentification.py +++ b/utils/robotIdentification.py @@ -2,7 +2,7 @@ #The constants between practice and main robots may be different. from enum import Enum import wpilib -#from utils.signalLogging import log +#from utils.signalLogging import addLog from utils.singleton import Singleton RobotTypes = Enum('RobotTypes', ['Main','Practice','TestBoard']) diff --git a/utils/segmentTimeTracker.py b/utils/segmentTimeTracker.py index 3cc7c7f..ef17a05 100644 --- a/utils/segmentTimeTracker.py +++ b/utils/segmentTimeTracker.py @@ -1,6 +1,6 @@ import wpilib -from utils.signalLogging import log +from utils.signalLogging import addLog # Utilties for tracking how long certain chunks of code take @@ -15,19 +15,30 @@ def __init__(self, longLoopThresh=0.53): self.curPeriod = 0 self.curLoopExecDur = 0 + addLog("LoopPeriod", lambda: (self.curPeriod * 1000.0), "ms") + addLog("LoopDuration", lambda: (self.curLoopExecDur * 1000.0), "ms") + def start(self): + """ + Mark the start of a periodic loop + """ self.tracer.clearEpochs() self.prevLoopStartTime = self.loopStartTime self.loopStartTime = wpilib.Timer.getFPGATimestamp() def mark(self, name): + """ + Mark an intermdeate step complete during a periodic loop + """ self.tracer.addEpoch(name) def end(self): + """ + Mark the end of a periodic loop + """ self.loopEndTime = wpilib.Timer.getFPGATimestamp() self.curPeriod = self.loopStartTime - self.prevLoopStartTime self.curLoopExecDur = self.loopEndTime - self.loopStartTime if self.curLoopExecDur > self.longLoopThresh: self.tracer.printEpochs() - log("LoopPeriod", self.curPeriod * 1000.0, "ms") - log("LoopDuration", self.curLoopExecDur * 1000.0, "ms") + diff --git a/utils/signalLogging.py b/utils/signalLogging.py index 5901d61..635af3c 100644 --- a/utils/signalLogging.py +++ b/utils/signalLogging.py @@ -89,9 +89,5 @@ def addLog(alias: str, value_getter: Callable[[], float], units=None) -> None: """ _singletonInst.newLogVal(alias, value_getter, units) - -def log(_, __, ___=None): - pass # temp till we're done cleaning up - def sigNameToNT4TopicName(name): return f"/{BASE_TABLE}/{name}" diff --git a/wrappers/wrapperedPulseWidthEncoder.py b/wrappers/wrapperedPulseWidthEncoder.py index 029bc8e..a2eb0bd 100644 --- a/wrappers/wrapperedPulseWidthEncoder.py +++ b/wrappers/wrapperedPulseWidthEncoder.py @@ -1,7 +1,7 @@ import math from wpilib import DigitalInput, DutyCycle from utils.faults import Fault -from utils.signalLogging import log +from utils.signalLogging import addLog from utils.calibration import Calibration from utils.units import wrapAngleRad @@ -38,25 +38,32 @@ def __init__( self.maxPulseTimeSec = maxPulse self.minAcceptableFreq = minAcceptableFreq + 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}_angle", lambda: self.curAngleRad, "rad") + def update(self): """Return the raw angle reading from the sensor in radians""" - freq = self.dutyCycle.getFrequency() + self.freq = self.dutyCycle.getFrequency() self.faulted = ( - freq < self.minAcceptableFreq + self.freq < self.minAcceptableFreq ) # abnormal frequency, we must be faulted self.disconFault.set(self.faulted) if self.faulted: # Faulted - don't do any processing - pulseTime = -1 + self.pulseTime = -1 rawAngle = 0.0 self.curAngleRad = 0.0 else: # Not-Faulted - read the raw angle from the pulse width - pulseTime = self.dutyCycle.getOutput() * (1.0 / freq) + self.pulseTime = self.dutyCycle.getOutput() * (1.0 / self.freq) rawAngle = ( ( - (pulseTime - self.minPulseTimeSec) + (self.pulseTime - self.minPulseTimeSec) / (self.maxPulseTimeSec - self.minPulseTimeSec) ) * 2 @@ -69,9 +76,6 @@ def update(self): self.curAngleRad = wrapAngleRad(rawAngle - self.mountOffsetCal.get()) - #log(f"{self.name}_freq", freq, "Hz") - #log(f"{self.name}_pulseTime", pulseTime, "sec") - #log(f"{self.name}_angle", self.curAngleRad, "rad") def getAngleRad(self): return self.curAngleRad diff --git a/wrappers/wrapperedSparkMax.py b/wrappers/wrapperedSparkMax.py index 9d48467..6e9d6dd 100644 --- a/wrappers/wrapperedSparkMax.py +++ b/wrappers/wrapperedSparkMax.py @@ -1,6 +1,6 @@ from rev import CANSparkMax, SparkMaxPIDController, REVLibError, CANSparkLowLevel from wpilib import TimedRobot -from utils.signalLogging import log +from utils.signalLogging import addLog from utils.units import rev2Rad, rad2Rev, radPerSec2RPM, RPM2RadPerSec from utils.faults import Fault import time @@ -37,6 +37,12 @@ def __init__(self, canID, name, brakeMode=False, currentLimitA=40.0): self.disconFault = Fault(f"Spark Max {name} ID {canID} disconnected") self.simActPos = 0 + self.desPos = 0 + self.desVel = 0 + self.desVolt = 0 + self.actPos = 0 + self.actVel = 0 + # Perform motor configuration, tracking errors and retrying until we have success retryCounter = 0 while not self.configSuccess and retryCounter < 10: @@ -66,9 +72,17 @@ def __init__(self, canID, name, brakeMode=False, currentLimitA=40.0): self.configSuccess = True time.sleep(0.1) - #self.configSuccess = True #Debug code - this may/will cause problems self.disconFault.set(not self.configSuccess) + addLog(self.name + "_outputCurrent", self.ctrl.getOutputCurrent, "A") + 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 + "_actPos", lambda: self.actPos, "rad") + addLog(self.name + "_actVel", lambda: self.actVel, "RPM") + + def setInverted(self, isInverted): if self.configSuccess: self.ctrl.setInverted(isInverted) @@ -89,8 +103,8 @@ def setPosCmd(self, posCmd, arbFF=0.0): self.simActPos = posCmd posCmdRev = rad2Rev(posCmd) - #log(self.name + "_desPos", posCmd, "Rad") - #log(self.name + "_cmdVoltage", arbFF, "V") + self.desPos = posCmd + self.desVolt = arbFF if self.configSuccess: err = self.pidCtrl.setReference( @@ -103,7 +117,6 @@ def setPosCmd(self, posCmd, arbFF=0.0): self.disconFault.set(err != REVLibError.kOk) - #log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") def setVelCmd(self, velCmd, arbFF=0.0): @@ -113,28 +126,24 @@ def setVelCmd(self, velCmd, arbFF=0.0): velCmd (float): motor desired shaft velocity in radians per second arbFF (int, optional): _description_. Defaults to 0. """ - velCmdRPM = radPerSec2RPM(velCmd) - #log(self.name + "_desVel", velCmdRPM, "RPM") - #log(self.name + "_cmdVoltage", arbFF, "V") + self.desVel = radPerSec2RPM(velCmd) + self.desVolt = arbFF if self.configSuccess: err = self.pidCtrl.setReference( - velCmdRPM, + self.desVel, CANSparkMax.ControlType.kVelocity, 0, arbFF, SparkMaxPIDController.ArbFFUnits.kVoltage, ) self.disconFault.set(err != REVLibError.kOk) - #log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") def setVoltage(self, outputVoltageVolts): - #log(self.name + "_cmdVoltage", outputVoltageVolts, "V") + self.desVolt = outputVoltageVolts if self.configSuccess: self.ctrl.setVoltage(outputVoltageVolts) - #log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") - #log(self.name + "_appliedOutput", self.ctrl.getAppliedOutput(), "%") def getMotorPositionRad(self): if(TimedRobot.isSimulation()): @@ -144,7 +153,7 @@ def getMotorPositionRad(self): pos = rev2Rad(self.encoder.getPosition()) else: pos = 0 - #log(self.name + "_motorActPos", pos, "rad") + self.actPos = pos return pos def getMotorVelocityRadPerSec(self): @@ -152,5 +161,5 @@ def getMotorVelocityRadPerSec(self): vel = self.encoder.getVelocity() else: vel = 0 - #log(self.name + "_motorActVel", vel, "RPM") + self.actVel = vel return RPM2RadPerSec(vel)