Fixed up can bus ID's, re-enabled position optimizatoin, and reduced …

…logigng for cpu load

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
 

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())

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)
 

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()

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):

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:

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()
+            debugpy.wait_for_client()
+

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
 
 
 

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

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())

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

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

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,13 +144,13 @@ 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):
         if self.configSuccess:
             vel = self.encoder.getVelocity()
         else:
             vel = 0
-        log(self.name + "_motorActVel", vel, "RPM")
+        #log(self.name + "_motorActVel", vel, "RPM")
         return RPM2RadPerSec(vel)