From bf5a8762b3feda028cf78a4237bc11d9b3a1ca6f Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Fri, 18 Oct 2024 16:43:27 -0500 Subject: [PATCH] Fixed up can bus ID's, re-enabled position optimizatoin, and reduced logigng for cpu load --- .../poseEstimation/drivetrainPoseTelemetry.py | 7 --- drivetrain/swerveModuleControl.py | 10 ++--- drivetrain/swerveModuleGainSet.py | 2 +- humanInterface/driverInterface.py | 14 +++--- humanInterface/ledControl.py | 2 +- navigation/repulsorFieldPlanner.py | 6 +-- robot.py | 18 +++++++- utils/constants.py | 45 +++++++------------ utils/powerMonitor.py | 7 +-- utils/rioMonitor.py | 14 +++--- utils/signalLogging.py | 16 +++---- wrappers/wrapperedPulseWidthEncoder.py | 6 +-- wrappers/wrapperedSparkMax.py | 30 ++++++------- 13 files changed, 84 insertions(+), 93 deletions(-) diff --git a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py index 6ead529..a35e359 100644 --- a/drivetrain/poseEstimation/drivetrainPoseTelemetry.py +++ b/drivetrain/poseEstimation/drivetrainPoseTelemetry.py @@ -86,13 +86,6 @@ def update(self, estPose:Pose2d, moduleAngles): self.rightCamPosePublisher.set(Pose3d(estPose).transformBy(ROBOT_TO_RIGHT_CAM)) self.frontCamPosePublisher.set(Pose3d(estPose).transformBy(ROBOT_TO_FRONT_CAM)) - log("DT Pose Est X", metersToFeet(estPose.X()), "ft") - log("DT Pose Est Y", metersToFeet(estPose.Y()), "ft") - log("DT Pose Est T", estPose.rotation().degrees(), "deg") - log("DT Pose Des X", metersToFeet(self.desPose.X()), "ft") - log("DT Pose Des Y", metersToFeet(self.desPose.Y()), "ft") - log("DT Pose Des T", self.desPose.rotation().degrees(), "deg") - def setWPITrajectory(self, trajIn): """Display a specific trajectory on the robot Field2d diff --git a/drivetrain/swerveModuleControl.py b/drivetrain/swerveModuleControl.py index 529d6f5..a8109d4 100644 --- a/drivetrain/swerveModuleControl.py +++ b/drivetrain/swerveModuleControl.py @@ -175,12 +175,10 @@ def update(self): self.actualPosition.angle = self.actualState.angle # Optimize our incoming swerve command to minimize motion - #self.optimizedDesiredState = SwerveModuleState.optimize( - # self.desiredState, self.actualState.angle - #) - # Skip optimization for now - self.optimizedDesiredState = self.desiredState - + self.optimizedDesiredState = SwerveModuleState.optimize( + self.desiredState, self.actualState.angle + ) + # Use a PID controller to calculate the voltage for the azimuth motor self.azmthCtrl.setSetpoint(self.optimizedDesiredState.angle.degrees()) # type: ignore self.azmthVoltage = self.azmthCtrl.calculate(self.actualState.angle.degrees()) diff --git a/drivetrain/swerveModuleGainSet.py b/drivetrain/swerveModuleGainSet.py index 931138c..711c2e4 100644 --- a/drivetrain/swerveModuleGainSet.py +++ b/drivetrain/swerveModuleGainSet.py @@ -23,7 +23,7 @@ def __init__(self): "Drivetrain Module Wheel kV", 12.0 / RPM2RadPerSec(4700), "volts/radPerSec" ) self.wheelS = Calibration("Drivetrain Module Wheel kS", 0.12, "volts") - self.azmthP = Calibration("Drivetrain Module Azmth kP", 0.02) + self.azmthP = Calibration("Drivetrain Module Azmth kP", 0.115) self.azmthI = Calibration("Drivetrain Module Azmth kI", 0.0) self.azmthD = Calibration("Drivetrain Module Azmth kD", 0.0000) diff --git a/humanInterface/driverInterface.py b/humanInterface/driverInterface.py index 1f84f18..76806ff 100644 --- a/humanInterface/driverInterface.py +++ b/humanInterface/driverInterface.py @@ -79,13 +79,13 @@ 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") + #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/humanInterface/ledControl.py b/humanInterface/ledControl.py index c023316..e05f488 100644 --- a/humanInterface/ledControl.py +++ b/humanInterface/ledControl.py @@ -6,7 +6,7 @@ BLINK = -1.0 GREEN = 0.35 RED = 0.03 -BLUE = 0.85 +BLUE = 0.75 OFF = 0.0 class LEDControl(metaclass=Singleton): diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index b8aacd0..cc680de 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -293,9 +293,9 @@ def updateTelemetry(self) -> list[Pose2d]: This includes logging data Returns the list of Pose2D's that are the result of the lookahead operation """ - 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") + #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 caffc5f..83e54cd 100644 --- a/robot.py +++ b/robot.py @@ -1,4 +1,5 @@ import sys +import cProfile, pstats import wpilib from dashboard import Dashboard from drivetrain.controlStrategies.autoDrive import AutoDrive @@ -19,7 +20,7 @@ from webserver.webserver import Webserver from AutoSequencerV2.autoSequencer import AutoSequencer from utils.powerMonitor import PowerMonitor -from wpimath.geometry import Translation2d +from wpimath.geometry import Translation2d, Pose2d, Rotation2d class MyRobot(wpilib.TimedRobot): ######################################################### @@ -57,6 +58,8 @@ def robotInit(self): self.addPeriodic(CalibrationWrangler().update, 0.5, 0.0) self.addPeriodic(FaultWrangler().update, 0.2, 0.0) + self.autoHasRun = False + def robotPeriodic(self): self.stt.start() @@ -86,6 +89,9 @@ def autonomousInit(self): # Use the autonomous rouines starting pose to init the pose estimator self.driveTrain.poseEst.setKnownPose(self.autoSequencer.getStartingPose()) + # Mark we at least started autonomous + self.autoHasRun = True + def autonomousPeriodic(self): SignalWrangler().markLoopStart() @@ -103,6 +109,13 @@ def teleopInit(self): # clear existing telemetry trajectory 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 + if not self.autoHasRun: + self.driveTrain.poseEst.setKnownPose( + Pose2d(1.0, 1.0, Rotation2d(0.0)) + ) + def teleopPeriodic(self): @@ -169,4 +182,5 @@ def remoteRIODebugSupport(): pass else: debugpy.listen(("0.0.0.0", 5678)) - debugpy.wait_for_client() \ No newline at end of file + debugpy.wait_for_client() + diff --git a/utils/constants.py b/utils/constants.py index a16f322..979278c 100644 --- a/utils/constants.py +++ b/utils/constants.py @@ -18,36 +18,21 @@ # Reserved_CANID = 0 # Reserved_CANID = 1 -DT_FL_WHEEL_CANID = 2 -DT_FL_AZMTH_CANID = 3 -DT_FR_WHEEL_CANID = 4 -DT_FR_AZMTH_CANID = 5 -DT_BL_WHEEL_CANID = 6 -DT_BL_AZMTH_CANID = 7 -DT_BR_WHEEL_CANID = 9 -DT_BR_AZMTH_CANID = 8 -CLIMBER_MOTOR_RIGHT_CANID = 10 -CLIMBER_MOTOR_LEFT_CANID = 11 -SHOOTER_MOTOR_RIGHT_CANID = 12 -ELEVATOR_TOF_CANID = 13 -INTAKE_MOTOR_LOWER_CANID = 14 -INTAKE_MOTOR_UPPER_CANID = 15 -GAMEPIECE_HANDLING_TOF_CANID = 16 -# Unused = 17 -ELEVATOR_HEIGHT_LEFT_MOTOR_CANID = 18 -ELEVATOR_HEIGHT_RIGHT_MOTOR_CANID = 19 -SINGER_ANGLE_MOTOR_CANID = 20 -FLOORROLLER_MOTOR1_CANID = 21 # dont know yet -# UNUSED = 22 -SHOOTER_MOTOR_LEFT_CANID = 23 -# Unused = 24 -# Unused = 25 -# Unused = 26 -# Unused = 27 -# Unused = 28 -# Unused = 29 -# Unused = 30 -# Unused = 31 +DT_FR_WHEEL_CANID = 2 +DT_FR_AZMTH_CANID = 3 +DT_FL_WHEEL_CANID = 4 +DT_FL_AZMTH_CANID = 5 +DT_BR_WHEEL_CANID = 6 +DT_BR_AZMTH_CANID = 7 +DT_BL_AZMTH_CANID = 8 +DT_BL_WHEEL_CANID = 9 +# Unused_CANID = 10 +# Unused_CANID = 11 +# Unused_CANID = 12 +# Unused_CANID = 13 +# Unused_CANID = 14 +# Unused_CANID = 15 +# Unused_CANID = 16 diff --git a/utils/powerMonitor.py b/utils/powerMonitor.py index 19bbadb..83509fb 100644 --- a/utils/powerMonitor.py +++ b/utils/powerMonitor.py @@ -62,6 +62,7 @@ def __init__(self): self.powerDist = wpilib.PowerDistribution() 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") + #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 e65bbde..2c56b3e 100644 --- a/utils/rioMonitor.py +++ b/utils/rioMonitor.py @@ -73,15 +73,15 @@ def _updateDiskStats(self): 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", - ) + #log("RIO CAN Bus Usage", status.percentBusUtilization, "pct") + #log( + # "RIO CAN Bus Err Count", + # status.txFullCount + status.receiveErrorCount + status.transmitErrorCount, + # "count", + #) def _updateVoltages(self): - log("RIO Supply Voltage", RobotController.getInputVoltage(), "V") + #log("RIO Supply Voltage", RobotController.getInputVoltage(), "V") if not RobotController.isBrownedOut(): self.railFault3p3v.set(not RobotController.getEnabled3V3()) self.railFault5v.set(not RobotController.getEnabled5V()) diff --git a/utils/signalLogging.py b/utils/signalLogging.py index 4b4a712..669bc3a 100644 --- a/utils/signalLogging.py +++ b/utils/signalLogging.py @@ -17,14 +17,14 @@ def __init__(self): self.publishedSigDict = {} self.fileLogging = False self.time = int(0) - - if ExtDriveManager().isConnected(): - wpilib.DataLogManager.start(dir=ExtDriveManager().getLogStoragePath()) - wpilib.DataLogManager.logNetworkTables( - False - ) # We have a lot of things in NT that don't need to be logged - self.log = wpilib.DataLogManager.getLog() - self.fileLogging = True + + #if ExtDriveManager().isConnected(): + # wpilib.DataLogManager.start(dir=ExtDriveManager().getLogStoragePath()) + # wpilib.DataLogManager.logNetworkTables( + # False + # ) # We have a lot of things in NT that don't need to be logged + # self.log = wpilib.DataLogManager.getLog() + # self.fileLogging = True def markLoopStart(self): self.time = nt._now() # pylint: disable=W0212 diff --git a/wrappers/wrapperedPulseWidthEncoder.py b/wrappers/wrapperedPulseWidthEncoder.py index 60e293e..029bc8e 100644 --- a/wrappers/wrapperedPulseWidthEncoder.py +++ b/wrappers/wrapperedPulseWidthEncoder.py @@ -69,9 +69,9 @@ 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") + #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 41f4487..9d48467 100644 --- a/wrappers/wrapperedSparkMax.py +++ b/wrappers/wrapperedSparkMax.py @@ -7,10 +7,10 @@ _StatusFramePeriodConfigs = [ - [CANSparkMax.PeriodicFrame.kStatus0, 20], # Status 0 = Motor output and Faults + [CANSparkMax.PeriodicFrame.kStatus0, 200], # Status 0 = Motor output and Faults [ CANSparkMax.PeriodicFrame.kStatus1, - 20, + 100, ], # Status 1 = Motor velocity & electrical data [CANSparkMax.PeriodicFrame.kStatus2, 20], # Status 2 = Motor Position [CANSparkMax.PeriodicFrame.kStatus3, 32767], # Status 3 = Analog Sensor Input @@ -66,7 +66,7 @@ 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.configSuccess = True #Debug code - this may/will cause problems self.disconFault.set(not self.configSuccess) def setInverted(self, isInverted): @@ -89,8 +89,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") + #log(self.name + "_desPos", posCmd, "Rad") + #log(self.name + "_cmdVoltage", arbFF, "V") if self.configSuccess: err = self.pidCtrl.setReference( @@ -103,7 +103,7 @@ def setPosCmd(self, posCmd, arbFF=0.0): self.disconFault.set(err != REVLibError.kOk) - log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") + #log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") def setVelCmd(self, velCmd, arbFF=0.0): @@ -115,8 +115,8 @@ def setVelCmd(self, velCmd, arbFF=0.0): """ velCmdRPM = radPerSec2RPM(velCmd) - log(self.name + "_desVel", velCmdRPM, "RPM") - log(self.name + "_cmdVoltage", arbFF, "V") + #log(self.name + "_desVel", velCmdRPM, "RPM") + #log(self.name + "_cmdVoltage", arbFF, "V") if self.configSuccess: err = self.pidCtrl.setReference( @@ -127,14 +127,14 @@ def setVelCmd(self, velCmd, arbFF=0.0): SparkMaxPIDController.ArbFFUnits.kVoltage, ) self.disconFault.set(err != REVLibError.kOk) - log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") + #log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") def setVoltage(self, outputVoltageVolts): - log(self.name + "_cmdVoltage", outputVoltageVolts, "V") + #log(self.name + "_cmdVoltage", outputVoltageVolts, "V") if self.configSuccess: - err = self.ctrl.setVoltage(outputVoltageVolts) - #self.disconFault.set(err != REVLibError.kOk) This is commented out for debugging purposes. - log(self.name + "_outputCurrent", self.ctrl.getOutputCurrent(), "A") + 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 +144,7 @@ def getMotorPositionRad(self): pos = rev2Rad(self.encoder.getPosition()) else: pos = 0 - log(self.name + "_motorActPos", pos, "rad") + #log(self.name + "_motorActPos", pos, "rad") return pos def getMotorVelocityRadPerSec(self): @@ -152,5 +152,5 @@ def getMotorVelocityRadPerSec(self): vel = self.encoder.getVelocity() else: vel = 0 - log(self.name + "_motorActVel", vel, "RPM") + #log(self.name + "_motorActVel", vel, "RPM") return RPM2RadPerSec(vel)