From 31e8c6965e9f5483925b6c1073ffa1f21259441b Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Sun, 20 Oct 2024 08:57:21 -0500 Subject: [PATCH] one million bugfixes and comments --- Autonomous/commands/drivePathCommand.py | 2 +- drivetrain/controlStrategies/autoDrive.py | 5 +- .../holonomicDriveController.py | 33 ++--- drivetrain/controlStrategies/trajectory.py | 4 +- drivetrain/drivetrainCommand.py | 7 +- drivetrain/drivetrainControl.py | 2 +- .../poseEstimation/drivetrainPoseEstimator.py | 116 +++++++++++------- drivetrain/swerveModuleControl.py | 62 +++++++--- drivetrain/swerveModuleGainSet.py | 3 +- humanInterface/driverInterface.py | 54 ++++---- humanInterface/ledControl.py | 23 +++- jormungandr/__init__.py | 6 + jormungandr/choreo.py | 5 + jormungandr/choreoTrajectory.py | 79 ++++++++++-- navigation/__init__.py | 4 + navigation/forceGenerators.py | 2 +- navigation/navConstants.py | 6 - navigation/navForce.py | 3 +- navigation/repulsorFieldPlanner.py | 2 +- robot.py | 10 +- utils/__init__.py | 4 + utils/allianceTransformUtils.py | 12 +- utils/constants.py | 17 ++- utils/crashLogger.py | 69 ++++++----- utils/extDriveManager.py | 8 +- utils/faults.py | 20 ++- utils/fieldTagLayout.py | 19 ++- utils/functionGenerator.py | 5 + utils/mapLookup2d.py | 10 +- utils/mathUtils.py | 6 +- utils/powerMonitor.py | 4 +- utils/rioMonitor.py | 10 +- utils/robotIdentification.py | 41 ++++--- utils/segmentTimeTracker.py | 7 +- utils/signalLogging.py | 11 +- utils/singleton.py | 22 ++-- utils/units.py | 22 ++-- wrappers/wrapperedPulseWidthEncoder.py | 2 +- 38 files changed, 485 insertions(+), 232 deletions(-) create mode 100644 navigation/__init__.py diff --git a/Autonomous/commands/drivePathCommand.py b/Autonomous/commands/drivePathCommand.py index c87bcf4..1d6ed30 100644 --- a/Autonomous/commands/drivePathCommand.py +++ b/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() diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 926b436..ba01cd2 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/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") diff --git a/drivetrain/controlStrategies/holonomicDriveController.py b/drivetrain/controlStrategies/holonomicDriveController.py index ff53fc1..55baf00 100644 --- a/drivetrain/controlStrategies/holonomicDriveController.py +++ b/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( diff --git a/drivetrain/controlStrategies/trajectory.py b/drivetrain/controlStrategies/trajectory.py index e6b733c..d7dd407 100644 --- a/drivetrain/controlStrategies/trajectory.py +++ b/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 diff --git a/drivetrain/drivetrainCommand.py b/drivetrain/drivetrainCommand.py index 372f44a..e49448f 100644 --- a/drivetrain/drivetrainCommand.py +++ b/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 diff --git a/drivetrain/drivetrainControl.py b/drivetrain/drivetrainControl.py index 5ed2ce6..6e2a205 100644 --- a/drivetrain/drivetrainControl.py +++ b/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) diff --git a/drivetrain/poseEstimation/drivetrainPoseEstimator.py b/drivetrain/poseEstimation/drivetrainPoseEstimator.py index eafe882..e9cb0b2 100644 --- a/drivetrain/poseEstimation/drivetrainPoseEstimator.py +++ b/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())) \ No newline at end of file + def _getGyroAngle(self)->Rotation2d: + return Rotation2d().fromDegrees(self._gyro.getAngle(self._gyro.getYawAxis())) \ No newline at end of file diff --git a/drivetrain/swerveModuleControl.py b/drivetrain/swerveModuleControl.py index 14a7023..7102406 100644 --- a/drivetrain/swerveModuleControl.py +++ b/drivetrain/swerveModuleControl.py @@ -6,17 +6,15 @@ from wpimath.kinematics import SwerveModulePosition from wpimath.geometry import Rotation2d from wpimath.filter import SlewRateLimiter -import wpilib +from wpilib import TimedRobot +from drivetrain.swerveModuleGainSet import SwerveModuleGainSet from wrappers.wrapperedSparkMax import WrapperedSparkMax from wrappers.wrapperedSRXMagEncoder import WrapperedSRXMagEncoder from dashboardWidgets.swerveState import getAzmthDesTopicName, getAzmthActTopicName from dashboardWidgets.swerveState import getSpeedDesTopicName, getSpeedActTopicName from utils.signalLogging import addLog -from utils.units import rad2Deg -from utils.faults import Fault -from utils.robotIdentification import RobotIdentification from drivetrain.drivetrainPhysical import dtMotorRotToLinear from drivetrain.drivetrainPhysical import dtLinearToMotorRot from drivetrain.drivetrainPhysical import MAX_FWD_REV_SPEED_MPS @@ -24,18 +22,35 @@ class SwerveModuleControl: """ - Control logic for one swerve drive module + Control logic for one swerve drive module. + + Convention Reminders: + The **module** refers to the whole assembly, including two motors, their built-in sensors, + the azimuth angle sensor, the hardware, everything. + + The **azimuth** is the motor, sensor, and mechanism to point the wheel in a specific direction. + + Positive azimuth rotation is counter-clockwise when viewed top-down. By the right hand rule, this is + rotation in the positive-Z direction. Zero degrees is toward the front of the robot + + The **wheel** is the motor and mechanism to apply a force in that direction. + + Positive wheel rotation causes the robot to move forward if the azimuth is pointed forward. + + Uses WPILib convention for names: + 1) "State" refers to the speed of the wheel, plus the position of the azimuth + 2) "Position" refers to the position of the wheel, plus the position of the azimuth """ def __init__( self, - moduleName, - wheelMotorCanID, - azmthMotorCanID, - azmthEncoderPortIdx, - azmthOffset, - invertWheel, - invertAzmth + moduleName:str, + wheelMotorCanID:int, + azmthMotorCanID:int, + azmthEncoderPortIdx:int, + azmthOffset:float, + invertWheel:bool, + invertAzmth:bool ): """Instantiate one swerve drive module @@ -54,6 +69,10 @@ def __init__( self.azmthMotor = WrapperedSparkMax( azmthMotorCanID, moduleName + "_azmth", True ) + + # Note the azimuth encoder inversion should be fixed, based on the physical design of the encoder itself, + # plus the swerve module physical construction. It might need to be tweaked here though if we change + # module brands or sensor brands. self.azmthEnc = WrapperedSRXMagEncoder( azmthEncoderPortIdx, moduleName + "_azmthEnc", azmthOffset, False ) @@ -100,28 +119,28 @@ def __init__( # Simulation Support Only self.wheelSimFilter = SlewRateLimiter(24.0) - def getActualPosition(self): + def getActualPosition(self)->SwerveModulePosition: """ Returns: SwerveModulePosition: The position of the module (azmth and wheel) as measured by sensors """ return self.actualPosition - def getActualState(self): + def getActualState(self)->SwerveModuleState: """ Returns: SwerveModuleState: The state of the module (azmth and wheel) as measured by sensors """ return self.actualState - def getDesiredState(self): + def getDesiredState(self)->SwerveModuleState: """ Returns: SwerveModuleState: The commanded, desired state of the module (azmth and wheel) """ return self.desiredState - def setClosedLoopGains(self, gains): + def setClosedLoopGains(self, gains:SwerveModuleGainSet): """Set feed-forward and closed loop gains for the module Args: @@ -137,7 +156,7 @@ def setClosedLoopGains(self, gains): gains.azmthP.get(), gains.azmthI.get(), gains.azmthD.get() ) - def setDesiredState(self, desState): + def setDesiredState(self, desState:SwerveModuleState): """Main command input - Call this to tell the module to go to a certian wheel speed and azimuth angle Args: @@ -151,8 +170,8 @@ def update(self): # Read from the azimuth angle sensor (encoder) self.azmthEnc.update() - if wpilib.TimedRobot.isReal(): - # Real Robot + if TimedRobot.isReal(): + # Real Robot. Use the actual sensors to get data about the module. # Update this module's actual state with measurements from the sensors self.actualState.angle = Rotation2d(self.azmthEnc.getAngleRad()) self.actualState.speed = dtMotorRotToLinear( @@ -186,7 +205,10 @@ def update(self): self._prevMotorDesSpeed = motorDesSpd # save for next loop - if wpilib.TimedRobot.isSimulation(): + if TimedRobot.isSimulation(): + # Simulation only. Do a very rough simulation of module behavior, and populate + # sensor data for the next loop. + # Very simple voltage/motor model of azimuth rotation self.actualState.angle += Rotation2d.fromDegrees(self.azmthVoltage / 12.0 * 1000.0 * 0.02) self.actualPosition.angle = self.actualState.angle diff --git a/drivetrain/swerveModuleGainSet.py b/drivetrain/swerveModuleGainSet.py index 711c2e4..ad5b1ea 100644 --- a/drivetrain/swerveModuleGainSet.py +++ b/drivetrain/swerveModuleGainSet.py @@ -1,5 +1,4 @@ from utils.calibration import Calibration -from utils.robotIdentification import RobotIdentification, RobotTypes from utils.units import RPM2RadPerSec @@ -27,7 +26,7 @@ def __init__(self): self.azmthI = Calibration("Drivetrain Module Azmth kI", 0.0) self.azmthD = Calibration("Drivetrain Module Azmth kD", 0.0000) - def hasChanged(self): + def hasChanged(self)->bool: """ Returns: bool: True if any gain in the set is modified, false otherwise diff --git a/humanInterface/driverInterface.py b/humanInterface/driverInterface.py index d0a6807..e7c8efa 100644 --- a/humanInterface/driverInterface.py +++ b/humanInterface/driverInterface.py @@ -15,24 +15,33 @@ def __init__(self): # contoller ctrlIdx = 0 self.ctrl = XboxController(ctrlIdx) + self.connectedFault = Fault(f"Driver XBox controller ({ctrlIdx}) unplugged") + + # Drivetrain motion commands self.velXCmd = 0 self.velYCmd = 0 self.velTCmd = 0 - self.gyroResetCmd = False - self.connectedFault = Fault(f"Driver XBox Controller ({ctrlIdx}) Unplugged") + + # Driver motion rate limiters - enforce smoother driving self.velXSlewRateLimiter = SlewRateLimiter(rateLimit=MAX_TRANSLATE_ACCEL_MPS2) self.velYSlewRateLimiter = SlewRateLimiter(rateLimit=MAX_TRANSLATE_ACCEL_MPS2) self.velTSlewRateLimiter = SlewRateLimiter(rateLimit=MAX_ROTATE_ACCEL_RAD_PER_SEC_2) - self.navToSpeaker = False - self.navToPickup = False - self.createObstacle = False + # Navigation commands + self.autoDriveToSpeaker = False + self.autoDriveToPickup = False + self.createDebugObstacle = False + + # Utility - reset to zero-angle at the current pose + 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 navToSpeaker", lambda: self.navToSpeaker, "bool") - addLog("DI navToPickup", lambda: self.navToPickup, "bool") + addLog("DI autoDriveToSpeaker", lambda: self.autoDriveToSpeaker, "bool") + addLog("DI autoDriveToPickup", lambda: self.autoDriveToPickup, "bool") def update(self): # value of contoller buttons @@ -53,10 +62,11 @@ def update(self): vYJoyWithDeadband = applyDeadband(vYJoyRaw, 0.15) vRotJoyWithDeadband = applyDeadband(vRotJoyRaw, 0.2) + # TODO - if the driver wants a slow or sprint button, add it here. #slowMult = 1.0 if (self.ctrl.getRightBumper()) else 0.75 slowMult = 1.0 - # velocity cmd + # Shape velocity command velCmdXRaw = vXJoyWithDeadband * MAX_STRAFE_SPEED_MPS * slowMult velCmdYRaw = vYJoyWithDeadband * MAX_FWD_REV_SPEED_MPS * slowMult velCmdRotRaw = vRotJoyWithDeadband * MAX_ROTATE_SPEED_RAD_PER_SEC @@ -68,9 +78,9 @@ def update(self): self.gyroResetCmd = self.ctrl.getAButton() - self.navToSpeaker = self.ctrl.getBButton() - self.navToPickup = self.ctrl.getXButton() - self.createObstacle = self.ctrl.getYButtonPressed() + self.autoDriveToSpeaker = self.ctrl.getBButton() + self.autoDriveToPickup = self.ctrl.getXButton() + self.createDebugObstacle = self.ctrl.getYButtonPressed() self.connectedFault.setNoFault() @@ -80,29 +90,29 @@ def update(self): self.velYCmd = 0.0 self.velTCmd = 0.0 self.gyroResetCmd = False - self.navToSpeaker = False - self.navToPickup = False + self.autoDriveToSpeaker = False + self.autoDriveToPickup = False + self.createDebugObstacle = False self.connectedFault.setFaulted() - self.createObstacle = False - def getCmd(self): + def getCmd(self) -> DrivetrainCommand: retval = DrivetrainCommand() retval.velX = self.velXCmd retval.velY = self.velYCmd retval.velT = self.velTCmd return retval - def getNavToSpeaker(self): - return self.navToSpeaker + def getNavToSpeaker(self) -> bool: + return self.autoDriveToSpeaker - def getNavToPickup(self): - return self.navToPickup + def getNavToPickup(self) -> bool: + return self.autoDriveToPickup - def getGyroResetCmd(self): + def getGyroResetCmd(self) -> bool: return self.gyroResetCmd - def getCreateObstacle(self): - return self.createObstacle \ No newline at end of file + def getCreateObstacle(self) -> bool: + return self.createDebugObstacle \ No newline at end of file diff --git a/humanInterface/ledControl.py b/humanInterface/ledControl.py index e05f488..110ccfa 100644 --- a/humanInterface/ledControl.py +++ b/humanInterface/ledControl.py @@ -10,15 +10,26 @@ OFF = 0.0 class LEDControl(metaclass=Singleton): + """ + Simple control class for the small, custom LED boards that we've used for a few years now. + Those LED boards are designed to take in a PWM signal, just like a motor controller. + Zero is off, increasing magnitidue cycles through different hue's. + Positive numbers are solid, negative numbers are blinky. + + See https://github.com/gerth2/MiniLEDBar/tree/v2_casserole_light_board for source code for the light bar itself + See https://oshwlab.com/chrisgerth010592/miniprogrammablelightbar_copy for hardware info + """ def __init__(self): self._isAutoDrive = False self._isStuck = False self.stuckDebounce = Debouncer(0.3, Debouncer.DebounceType.kFalling) - self.ctrlBlock = PWMMotorController("LEDCtrl", LED_STACK_LIGHT_CTRL_PWM) + self.ledPWMOutput = PWMMotorController("LEDCtrl", LED_STACK_LIGHT_CTRL_PWM) def update(self): - + """ + Pick a color to send to the LED based on the robot's status + """ stuckDebounced = self.stuckDebounce.calculate(self._isStuck) if(self._isAutoDrive): @@ -29,10 +40,16 @@ def update(self): else: pwmVal = GREEN - self.ctrlBlock.set(pwmVal) + self.ledPWMOutput.set(pwmVal) def setAutoDrive(self, isAutoDrive:bool): + """ + Set whether the LED should change color to indicate we are doing auto-drive now + """ self._isAutoDrive = isAutoDrive def setStuck(self, isStuck:bool): + """ + Set whether the LED should change color to indicate we are stuck while auto-driving + """ self._isStuck = isStuck \ No newline at end of file diff --git a/jormungandr/__init__.py b/jormungandr/__init__.py index d5b258f..b731d16 100644 --- a/jormungandr/__init__.py +++ b/jormungandr/__init__.py @@ -1 +1,7 @@ # No one here but us chickens + +""" +Choreo is developed by the Sleipnir Group, named for Oden's eight-legged horse. https://en.wikipedia.org/wiki/Sleipnir +In Norse mythology, Jörmungandr is a large snake. See https://en.wikipedia.org/wiki/J%C3%B6rmungandr +Chris thought it would be silly to continue the norse mythology references when he named this python package. +""" \ No newline at end of file diff --git a/jormungandr/choreo.py b/jormungandr/choreo.py index a4c3815..4a1bd07 100644 --- a/jormungandr/choreo.py +++ b/jormungandr/choreo.py @@ -4,6 +4,11 @@ def fromFile(file: str) -> ChoreoTrajectory: + """ + Handles reading a choreo trajectory file from disk, + reading all the data into it, and creating the list of + trajectory states that we'll later use to control the robot + """ samples = [] with open(file, "r", encoding="utf-8") as trajFile: data = json.load(trajFile) diff --git a/jormungandr/choreoTrajectory.py b/jormungandr/choreoTrajectory.py index 5765ff2..15a4c7e 100644 --- a/jormungandr/choreoTrajectory.py +++ b/jormungandr/choreoTrajectory.py @@ -3,17 +3,22 @@ from wpimath.geometry import Pose2d, Rotation2d from wpimath.kinematics import ChassisSpeeds -from utils.constants import FIELD_LENGTH_FT +from utils.constants import FIELD_X_M -from utils.units import ft2m - - -# Doesn't appear to be pulled into python from Interpolatable def _floatInterp(start, end, frac) -> float: + """ + Note, in theory wpilib should have this, but the python version in 2024 + didn't seem to match up with what java/c++ had added - not sure why it + was missing. It was simple enough we just replicated it here though. + """ return start + (end - start) * frac class ChoreoTrajectoryState: + """ + Represents what state the robot should be in at a particular time. + State includes both position and velocity in all dimensions + """ def __init__( self, timestamp: float, @@ -33,12 +38,22 @@ def __init__( self.angularVelocity = angularVelocity def getPose(self) -> Pose2d: + """ + Get a pose object associated with this state + """ return Pose2d(self.x, self.y, Rotation2d(value=self.heading)) def getChassisSpeeds(self) -> ChassisSpeeds: + """ + Get a chassis speed associated with this object + """ return ChassisSpeeds(self.velocityX, self.velocityY, self.angularVelocity) def asArray(self) -> list[float]: + """ + Dump all values into an array in a specific order. This is mostly + for telemetry purposes. + """ return [ self.timestamp, self.x, @@ -52,6 +67,10 @@ def asArray(self) -> list[float]: def interpolate( self, endValue: ChoreoTrajectoryState, t: float ) -> ChoreoTrajectoryState: + """ + Perform Linear Interpolation between this trajectory state, and some other one. + Useful for getting a state that's part way between two defined states. + """ scale = (t - self.timestamp) / (endValue.timestamp - self.timestamp) return ChoreoTrajectoryState( @@ -65,9 +84,15 @@ def interpolate( ) def flipped(self): + """ + Mirror the trajectory state to the other alliance. Note that this assumes the style of + symmetry that the 2023/2024 fields had. Additionally, note that this is kinda the same thing + that we did in allianceTransformUtils, but the Choreo library also provided the functionality. + It's here too. Depending on how Choreo architects in 2025+, we might want to move this. + """ return ChoreoTrajectoryState( self.timestamp, - ft2m(FIELD_LENGTH_FT) - self.x, + FIELD_X_M - self.x, self.y, math.pi - self.heading, self.velocityX * -1.0, @@ -77,17 +102,32 @@ def flipped(self): class ChoreoTrajectory: + """ + A trajectory is a series of states that describes how the robot moves through + a pre-defined path on the field. Choreo is a tool that generates "optimal" paths + based on knowledge of what a swerve drive can physically do. These are designed + and generated on a laptop. Then, before autonomous, the robot loads the + trajectory and sends commands to the drivetrain based on the trajectory + """ def __init__(self, samples: list[ChoreoTrajectoryState]): self.samples = samples def _sampleImpl(self, timestamp) -> ChoreoTrajectoryState: + """ + Actual implementation of getting the robot state at a specific timestamp + """ + # Handle timestamps outside the trajectory range if timestamp < self.samples[0].timestamp: return self.samples[0] if timestamp > self.getTotalTime(): return self.samples[-1] + # Binary search to find the two states on either side of the requested timestamps + # TODO - We might be able to pick better low/high guesses. We know we'll almost always + # be iterating throught the trajectory in roughly 20ms steps. It would be helpful to + # not have to search the full array each time. low = 0 high = len(self.samples) - 1 while low != high: @@ -115,25 +155,44 @@ def _sampleImpl(self, timestamp) -> ChoreoTrajectoryState: def sample( self, timestamp: float, mirrorForRedAlliance: bool = False ) -> ChoreoTrajectoryState: + """ + Return the desired state of the robot at the specified time + """ tmp = self._sampleImpl(timestamp) + # Include flipping for alliance if needed. if mirrorForRedAlliance: return tmp.flipped() else: # no mirroring return tmp - def getInitialPose(self): + def getInitialPose(self) -> Pose2d: + """ + Returns the pose we assume the robot starts the trajectory at. + """ return self.samples[0].getPose() - def getFinalPose(self): + def getFinalPose(self) -> Pose2d: + """ + Returns the pose we hope the robot will end the trajectory at + """ return self.samples[-1].getPose() - def getTotalTime(self): + def getTotalTime(self) -> float: + """ + Return the duration of the trajectory in seconds + """ return self.samples[-1].timestamp - def getPoses(self): + def getPoses(self) -> list[Pose2d]: + """ + Return the list of all poses associated with this trajectory. Mostly useful for telemetry. + """ return [x.getPose() for x in self.samples] def flipped(self) -> ChoreoTrajectory: + """ + Return the whole trajectory flipped to the opposite alliance. + """ return ChoreoTrajectory([x.flipped() for x in self.samples]) diff --git a/navigation/__init__.py b/navigation/__init__.py new file mode 100644 index 0000000..38735c0 --- /dev/null +++ b/navigation/__init__.py @@ -0,0 +1,4 @@ +""" +The Navigation package includes logic for the robot to self-plan a route through +the field, avoiding obstacles as it goes. +""" \ No newline at end of file diff --git a/navigation/forceGenerators.py b/navigation/forceGenerators.py index 1120958..f19abcf 100644 --- a/navigation/forceGenerators.py +++ b/navigation/forceGenerators.py @@ -1,6 +1,6 @@ import math from wpimath.geometry import Translation2d -from navigation.navConstants import FIELD_X_M, FIELD_Y_M +from utils.constants import FIELD_X_M, FIELD_Y_M from navigation.navForce import Force, logisticFunc class ForceGenerator: diff --git a/navigation/navConstants.py b/navigation/navConstants.py index 1f29c1f..6a76235 100644 --- a/navigation/navConstants.py +++ b/navigation/navConstants.py @@ -4,12 +4,6 @@ Constants related to navigation """ -""" -"Official" field dimensions -""" -FIELD_X_M = 16.54 -FIELD_Y_M = 8.21 - # Happy Constants for the goal poses we may want to drive to GOAL_PICKUP = Pose2d.fromFeet(40,5,Rotation2d.fromDegrees(0.0)) diff --git a/navigation/navForce.py b/navigation/navForce.py index dcbec1e..f2c1b48 100644 --- a/navigation/navForce.py +++ b/navigation/navForce.py @@ -16,7 +16,8 @@ def logisticFunc(x, L, k, x0): @dataclass class Force: """ - Simple class to represent a force in a 2d plane + Simple class to represent a force in a 2d plane. Represents the metaphorical "forces" + we use to perform on-the-fly path generation """ x:float=0 y:float=0 diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 2a02c8a..80a3677 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -7,7 +7,7 @@ from navigation.navForce import Force from navigation.forceGenerators import HorizontalObstacle, ForceGenerator, PointObstacle, VerticalObstacle -from navigation.navConstants import FIELD_X_M, FIELD_Y_M +from utils.constants import FIELD_X_M, FIELD_Y_M # Relative strength of how hard the goal pulls the robot toward it # Too big and the robot will be pulled through obstacles diff --git a/robot.py b/robot.py index 6cd4489..2844425 100644 --- a/robot.py +++ b/robot.py @@ -69,9 +69,9 @@ def robotPeriodic(self): 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.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()) @@ -109,7 +109,7 @@ def autonomousExit(self): ## Teleop-Specific init and update def teleopInit(self): # clear existing telemetry trajectory - self.driveTrain.poseEst.telemetry.setWPITrajectory(None) + self.driveTrain.poseEst._telemetry.setWPITrajectory(None) # If we're starting teleop but haven't run auto, set a nominal default pose # This is needed because initial pose is usually set by the autonomous routine @@ -153,7 +153,7 @@ def teleopPeriodic(self): ## Disabled-Specific init and update def disabledPeriodic(self): self.autoSequencer.updateMode() - Trajectory().trajCtrl.updateCals() + Trajectory().trajHDC.updateCals() def disabledInit(self): self.autoSequencer.updateMode(True) diff --git a/utils/__init__.py b/utils/__init__.py index d5b258f..5b63e6d 100644 --- a/utils/__init__.py +++ b/utils/__init__.py @@ -1 +1,5 @@ # No one here but us chickens + +""" +Miscelaneous utility classes +""" \ No newline at end of file diff --git a/utils/allianceTransformUtils.py b/utils/allianceTransformUtils.py index 7ed17b7..29e762f 100644 --- a/utils/allianceTransformUtils.py +++ b/utils/allianceTransformUtils.py @@ -2,13 +2,15 @@ import wpilib from wpimath.geometry import Pose2d, Rotation2d, Transform2d, Translation2d from jormungandr.choreoTrajectory import ChoreoTrajectoryState -from utils.constants import FIELD_LENGTH_FT -from utils.units import ft2m +from utils.constants import FIELD_X_M """ Utilities to help transform from blue alliance to red if needed We chose a coordinate system where the origin is always in the - bottom left on the blue alliance + bottom left on the blue alliance. + + Note that this assumes the "mirroring" that happened in 2023 and 2024. + A diagonally symmetric field (like 2020) would not need this. """ @@ -21,7 +23,7 @@ def onRed(): # Base transform for X axis to flip to the other side of the field. def transformX(xIn): if onRed(): - return ft2m(FIELD_LENGTH_FT) - xIn + return FIELD_X_M - xIn else: return xIn @@ -53,7 +55,7 @@ def transform(valIn: ChoreoTrajectoryState) -> ChoreoTrajectoryState: pass -# Actual implementation of the trasnform function +# Actual implementation of the transform function def transform(valIn): if isinstance(valIn, Rotation2d): if onRed(): diff --git a/utils/constants.py b/utils/constants.py index 979278c..729af33 100644 --- a/utils/constants.py +++ b/utils/constants.py @@ -5,12 +5,9 @@ ####################################################################################### ## FIELD DIMENSIONS ####################################################################################### -FIELD_LENGTH_FT = 54.2685 -SPEAKER_TARGET_HEIGHT_M = 2.0385024 -AMP_LOC_X_M = 1.84 -AMP_LOC_Y_M = 8.199 -SPEAKER_LOC_X_M = 0.22987 -SPEAKER_LOC_Y_M = 5.4572958333417994 + +FIELD_X_M = 16.54 # "Length" +FIELD_Y_M = 8.21 # "Width" ####################################################################################### ## CAN ID'S @@ -61,9 +58,9 @@ DT_FL_AZMTH_ENC_PORT = 1 DT_BL_AZMTH_ENC_PORT = 2 DT_FR_AZMTH_ENC_PORT = 3 -SINGER_ANGLE_ABS_POS_ENC = 4 -TUNER_ANGLE_ABS_POS_ENC = 5 +# Unused = 4 +# Unused = 5 # Unused = 6 # Unused = 7 -# Unused = 8 -# Unused = 9 +FIX_ME_LED_PIN = 8 +HEARTBEAT_LED_PIN = 9 diff --git a/utils/crashLogger.py b/utils/crashLogger.py index 8ae3322..f4d52e8 100644 --- a/utils/crashLogger.py +++ b/utils/crashLogger.py @@ -14,34 +14,6 @@ class CrashLogger: named log files on the USB drive for later retrieval """ - def update(self): - if ( - not self.prefixWritten - and wpilib.DriverStation.isFMSAttached() - and self.isRunning - ): - self.logPrint(f"==========================================") - self.logPrint(f"== FMS Data Received {datetime.now()}:") - self.logPrint(f"Event: {wpilib.DriverStation.getEventName()}") - self.logPrint(f"Match Type: {wpilib.DriverStation.getMatchType()}") - self.logPrint(f"Match Number: {wpilib.DriverStation.getMatchNumber()}") - self.logPrint(f"Replay Number: {wpilib.DriverStation.getReplayNumber()}") - self.logPrint( - f"Game Message: {wpilib.DriverStation.getGameSpecificMessage()}" - ) - self.logPrint(f"Cur FPGA Time: {wpilib.Timer.getFPGATimestamp()}") - self.logPrint(f"==========================================") - self.flushPrint() - self.prefixWritten = True - - - def logPrint(self, msg): - self.fileHandler.stream.write(msg) - self.fileHandler.stream.write("\n") - - def flushPrint(self): - self.fileHandler.stream.flush() - def __init__(self): self.prefixWritten = False self.isRunning = ExtDriveManager().isConnected() @@ -74,3 +46,44 @@ def __init__(self): self.logPrint(f"\n==============================================") self.logPrint(f"Beginning of Log {logPath}") self.logPrint(f"Started {datetime.now()}") + + + def update(self): + """ + Periodic log update function + """ + if ( + not self.prefixWritten + and wpilib.DriverStation.isFMSAttached() + and self.isRunning + ): + # One-time prefix write, which needs to wait until the FMS is attached. + # Once it is, dump the match info the log file for later retrieval. + self.logPrint(f"==========================================") + self.logPrint(f"== FMS Data Received {datetime.now()}:") + self.logPrint(f"Event: {wpilib.DriverStation.getEventName()}") + self.logPrint(f"Match Type: {wpilib.DriverStation.getMatchType()}") + self.logPrint(f"Match Number: {wpilib.DriverStation.getMatchNumber()}") + self.logPrint(f"Replay Number: {wpilib.DriverStation.getReplayNumber()}") + self.logPrint( + f"Game Message: {wpilib.DriverStation.getGameSpecificMessage()}" + ) + self.logPrint(f"Cur FPGA Time: {wpilib.Timer.getFPGATimestamp()}") + self.logPrint(f"==========================================") + self.flushPrint() + self.prefixWritten = True + + + def logPrint(self, msg:str): + """ + Print a message into the log, with a newline + """ + self.fileHandler.stream.write(msg) + self.fileHandler.stream.write("\n") + + def flushPrint(self): + """ + Flushes messages to disk. This is costly, but necessary if we think code is about + to crash, and want to ensure the file on disk actually has messages in it before crashing. + """ + self.fileHandler.stream.flush() diff --git a/utils/extDriveManager.py b/utils/extDriveManager.py index f48b856..d383f31 100644 --- a/utils/extDriveManager.py +++ b/utils/extDriveManager.py @@ -5,9 +5,15 @@ class ExtDriveManager(metaclass=Singleton): + """ + The External Drive Manager is responsible for checking whether an external USB drive is + available for logging purposes, and providing the path to it if so. Since the drive is + critical for debugging what happened during a match, a fault is raised if a drive is expected + but not detected to be writeable. + """ def __init__(self): self.enableDiskLogging = False - self.driveAvailableFault = Fault("Logging USB Drive Not Available") + self.driveAvailableFault = Fault("Logging USB drive not available") if wpilib.RobotBase.isSimulation(): # Disable in sim diff --git a/utils/faults.py b/utils/faults.py index 346ae72..c7bd1e5 100644 --- a/utils/faults.py +++ b/utils/faults.py @@ -1,11 +1,9 @@ import math import wpilib +from utils.constants import FIX_ME_LED_PIN, HEARTBEAT_LED_PIN from utils.singleton import Singleton -FIX_ME_LED_PIN = 8 -HEARTBEAT_LED_PIN = 9 - class FaultWrangler(metaclass=Singleton): """ @@ -51,6 +49,22 @@ def hasActiveFaults(self): class FaultStatusLEDs(metaclass=Singleton): + """ + Drives the two LED's on the robot maintenence panel based on current fault status. + "FIX-ME" -> Red -> blinks when one or more faults are active. + "HEARTBEAT" -> White -> pulses like a heart beat normally when code is running. + + Pit crew should be trained to look for a good heartbeat, AND no fix-me. + + No heartbeat means code isn't running. SW team needed to investigate. + Heartbeat AND fix-me means software detected hardware broken or disconnected. The driver dashboard should be pulled up to investigate the actual root cause. + + Admonition to software people: You must be very rigirous about not cerating false positives here. If + the fix-me light is on and blinking when the robot _doesn't_ need pit crew attention, they'll quickly + learn to just ignore it. And thereby ignore any real faults that get detected. + + Unless you want the pit crew to stop and fix the robot, DON'T blink the fix-me light. + """ def __init__(self): self.fixMeLED = wpilib.DigitalOutput(FIX_ME_LED_PIN) self.fixMeLED.setPWMRate(500.0) diff --git a/utils/fieldTagLayout.py b/utils/fieldTagLayout.py index 775e696..f0a37c1 100644 --- a/utils/fieldTagLayout.py +++ b/utils/fieldTagLayout.py @@ -1,10 +1,27 @@ from robotpy_apriltag import AprilTagField, loadAprilTagLayoutField from utils.singleton import Singleton +from wpimath.geometry import Pose3d class FieldTagLayout(metaclass=Singleton): + """ + Very thin wrapper around WPILib's apriltag field layout functions. But should expand in the future + """ def __init__(self): + """ + Performs a one-time load from file of the apriltag field layout .json + """ + + # TODO - this actually goes and accesses files under the hood. And... maybe... + # Throws exceptions if it has issues accessing the file. We should be catching + # those exceptions so the robot code doesn't outright crash, but raise a fault + # to indicate something has gone wrong with the RIO's ability to load the file. self.fieldTags = loadAprilTagLayoutField(AprilTagField.k2024Crescendo) - def lookup(self, tagId): + def lookup(self, tagId) -> Pose3d | None: + """ + Returns a Pose3d of a given tag's location in 3d space. Or, None if the id + isn't a tag we know a location for. + """ + # TODO - if the file couldn't be loaded successfully, this needs to return None return self.fieldTags.getTagPose(tagId) diff --git a/utils/functionGenerator.py b/utils/functionGenerator.py index 583ccb0..4cc074f 100644 --- a/utils/functionGenerator.py +++ b/utils/functionGenerator.py @@ -4,6 +4,11 @@ class FunctionGenerator: + """ + Class designed to create step and sine waveforms. + These are useful to use as commands to closed-loop control systems in test mode + as it's useful to tune PID controls around known, controllable waveforms. + """ def __init__(self, uniqueName): self.activeCal = Calibration(name="fg_" + uniqueName + "_active", default=0) self.typeCal = Calibration( diff --git a/utils/mapLookup2d.py b/utils/mapLookup2d.py index d97e5ac..01b0327 100644 --- a/utils/mapLookup2d.py +++ b/utils/mapLookup2d.py @@ -3,7 +3,15 @@ # pylint: disable=too-few-public-methods class MapLookup2D: - # points should be a list of two-element tuples, with (x,y) pairs described by each tuple. + """ + A map-lookup is a common tool to define a function. The user must provide samples of the + function's output value (y) at different inputs (x). This function will linearally interpolate + between the provided points. The asusmption is the function is "piecewise linear". As long + as enough sample points are provided, this method can well-aproximate just about any function. + + Points should be a list of two-element tuples, with (x,y) pairs described by each tuple. + + """ def __init__(self, points: list[tuple[float, float]]): self.points = points # Ensure points list is ordered from lowest value to highest value diff --git a/utils/mathUtils.py b/utils/mathUtils.py index 53c4db4..57a7cef 100644 --- a/utils/mathUtils.py +++ b/utils/mathUtils.py @@ -1,4 +1,8 @@ -def limit(inVal, maxMag): +def limit(inVal:float, maxMag:float)->float: + """ + Prevents a value from getting larger than some magnititude (positive or negative). + Returns the value if it's small enough, or the limit if it's too big. + """ if inVal > maxMag: return maxMag elif inVal < -1.0 * maxMag: diff --git a/utils/powerMonitor.py b/utils/powerMonitor.py index 5718f04..b204856 100644 --- a/utils/powerMonitor.py +++ b/utils/powerMonitor.py @@ -55,7 +55,9 @@ ''' class PowerMonitor: - + """ + Wrapper class to log electrical statistics on robot usage + """ def __init__(self): self.powerDist = wpilib.PowerDistribution() addLog("Battery current draw",self.powerDist.getTotalCurrent, "A") diff --git a/utils/rioMonitor.py b/utils/rioMonitor.py index edcc4a1..c8e9d58 100644 --- a/utils/rioMonitor.py +++ b/utils/rioMonitor.py @@ -8,12 +8,14 @@ from utils.signalLogging import addLog -# Records faults and runtime metrics for the roboRIO class RIOMonitor: def __init__(self): - self.railFault5v = Fault("RIO 5V (DIO) Rail Faulted") - self.railFault3p3v = Fault("RIO 3.3V Rail Faulted") - self.railFault6v = Fault("RIO 6V (PWM) Rail Faulted") + """ + Records faults and runtime metrics for the roboRIO. + """ + self.railFault5v = Fault("RIO 5V (DIO) rail faulted") + self.railFault3p3v = Fault("RIO 3.3V rail faulted") + self.railFault6v = Fault("RIO 6V (PWM) rail faulted") # CPU Stats - remember last time metrics self.prevUserTime = 0 diff --git a/utils/robotIdentification.py b/utils/robotIdentification.py index e4c44b1..14e7b18 100644 --- a/utils/robotIdentification.py +++ b/utils/robotIdentification.py @@ -1,40 +1,53 @@ -#The goal of this file is to identify which robot is currently running the code. -#The constants between practice and main robots may be different. from enum import Enum import wpilib -#from utils.signalLogging import addLog +from utils.faults import Fault from utils.singleton import Singleton + +""" +Specifc robots this codebase might run on. +""" RobotTypes = Enum('RobotTypes', ['Main','Practice','TestBoard']) class RobotIdentification(metaclass=Singleton): + """ + While we strive for our practice robot and main/competition robots to be as identical as possible, + that's not always the case. + The goal of this class is to identify which robot is currently running the code. + The constants between practice and main robots may be different. + """ def __init__(self): self.roboControl = wpilib.RobotController self.robotType = RobotTypes.Main - self.serialFault = False + self.serialFault = Fault("RoboRIO serial number not recognized") + self._configureValue() - def configureValue(self): + def _configureValue(self): - self.serialFault = False + self.serialFault.setNoFault() if self.roboControl.getSerialNumber() == "030e2cb0": + #Test to see if the RoboRio serial number is the main/"Production" bot. self.robotType = RobotTypes.Main elif self.roboControl.getSerialNumber() == "03064e3f": + #Test to see if the RoboRio serial number is the practice bot. self.robotType = RobotTypes.Practice elif self.roboControl.getSerialNumber() == "0316b37c": #Test to see if the RoboRio serial number is our testboard's serial number. self.robotType = RobotTypes.TestBoard else: # If the Robo Rio's serial number is not equal to any of our known serial numbers, - # assume we are the main robot + # assume we are the main robot. But, throw a fault, since this is something software + # team needs to fix. self.robotType = RobotTypes.Main - self.serialFault = True - - def getRobotType(self): - return self.robotType + self.serialFault.setFaulted() - def getRobotSerialNumber(self): + def _getRobotSerialNumber(self)->str: return self.roboControl.getSerialNumber() - def getSerialFaulted(self): - return self.serialFault \ No newline at end of file + def getRobotType(self)->RobotTypes: + """ + Return which robot we're running on right now + """ + return self.robotType + diff --git a/utils/segmentTimeTracker.py b/utils/segmentTimeTracker.py index ef17a05..639939f 100644 --- a/utils/segmentTimeTracker.py +++ b/utils/segmentTimeTracker.py @@ -3,9 +3,12 @@ from utils.signalLogging import addLog -# Utilties for tracking how long certain chunks of code take -# including logging overall loop execution time + class SegmentTimeTracker: + """ + Utilties for tracking how long certain chunks of code take + including logging overall loop execution time + """ def __init__(self, longLoopThresh=0.53): self.longLoopThresh = longLoopThresh self.tracer = wpilib.Tracer() diff --git a/utils/signalLogging.py b/utils/signalLogging.py index 7968349..99ef834 100644 --- a/utils/signalLogging.py +++ b/utils/signalLogging.py @@ -11,6 +11,12 @@ @dataclass class _LoggedVal(): + """ + Container for holding a data source (a callable in our code) + and all the places the data might go - NetworkTables for live publishing, + or a file for later review. If writing files is not possible (USB drive not in?) + the file publisher should be None + """ valGetter:Callable[[], float] ntPublisher:nt.DoublePublisher filePublisher:wpilog.DoubleLogEntry|None @@ -74,8 +80,11 @@ def newLogVal(self, name:str, valGetter:Callable[[],float], units:str|None): ########################################### _singletonInst = SignalWrangler() # cache a reference -# Log a new named value def logUpdate(): + """ + Periodic call to sample and broadcast all logged values. Should happen once per + 20ms loop. + """ _singletonInst.update() def addLog(alias: str, value_getter: Callable[[], float], units=None) -> None: diff --git a/utils/singleton.py b/utils/singleton.py index 36f9a4f..84fc686 100644 --- a/utils/singleton.py +++ b/utils/singleton.py @@ -1,16 +1,15 @@ -# Casserole Singleton Infrastructure -# Based on https://stackoverflow.com/q/6760685 - creating -# singletons with metaclasses. Namely, any class which should -# be a singleton should inherit `metaclass=Singleton` in its constructor -# On the first instantiaion, the single instance will be created and added -# to the global _instances dictionary - -# When the instance is destroyed, _instances = {} - class Singleton(type): + """ + Casserole Singleton Infrastructure + Based on https://stackoverflow.com/q/6760685 - creating + singletons with metaclasses. Namely, any class which should + be a singleton should inherit `metaclass=Singleton` in its constructor + On the first instantiaion, the single instance will be created and added + to the global _instances dictionary + """ def __call__(cls, *args, **kwargs): if cls not in _instances: _instances[cls] = super(Singleton, cls).__call__(*args, **kwargs) @@ -18,5 +17,10 @@ def __call__(cls, *args, **kwargs): def destroyAllSingletonInstances(): + """ + For unit testing purposes, we will need to simulate the roboRIO power cycling + One part of this is to get new instances of all the singletons constructed + This should never be called (nor NEED to be called) on the real robot. + """ global _instances _instances = {} diff --git a/utils/units.py b/utils/units.py index 858f87c..2249da9 100644 --- a/utils/units.py +++ b/utils/units.py @@ -4,48 +4,48 @@ ## Additional Unit conversion classes not in wpilib.units -def deg2Rad(inVal): +def deg2Rad(inVal:float)->float: return math.pi / 180.0 * inVal -def rad2Deg(inVal): +def rad2Deg(inVal:float)->float: return 180.0 / math.pi * inVal -def rev2Rad(inVal): +def rev2Rad(inVal:float)->float: return 2.0 * math.pi * inVal -def rad2Rev(inVal): +def rad2Rev(inVal:float)->float: return 1.0 / (math.pi * 2.0) * inVal -def m2ft(inVal): +def m2ft(inVal:float)->float: return 3.28084 * inVal -def ft2m(inVal): +def ft2m(inVal:float)->float: return float(inVal / 3.28084) -def m2in(inVal): +def m2in(inVal:float)->float: return 39.3701 * inVal -def in2m(inVal): +def in2m(inVal:float)->float: return inVal / 39.3701 -def radPerSec2RPM(inVal): +def radPerSec2RPM(inVal:float)->float: return inVal * 9.55 # pylint: disable=invalid-name -def RPM2RadPerSec(inVal): +def RPM2RadPerSec(inVal:float)->float: return inVal / 9.55 -def wrapAngleDeg(angle): +def wrapAngleDeg(angle:float)->float: angle %= 360.0 angle = (angle - 360) if angle > 180 else angle angle = (angle + 360) if angle < -180 else angle diff --git a/wrappers/wrapperedPulseWidthEncoder.py b/wrappers/wrapperedPulseWidthEncoder.py index a2eb0bd..82c2947 100644 --- a/wrappers/wrapperedPulseWidthEncoder.py +++ b/wrappers/wrapperedPulseWidthEncoder.py @@ -26,7 +26,7 @@ def __init__( ): self.dutyCycle = DutyCycle(DigitalInput(port)) self.name = f"Encoder_{name}" - self.disconFault = Fault(f"{self.name} DIO port {port} Disconnected") + self.disconFault = Fault(f"{self.name} DIO port {port} disconnected") self.mountOffsetCal = Calibration( self.name + "_mountOffset", mountOffsetRad, "rad" )