Reduced logging and bugfixes. Now detecting obstacles in about the ri…

…ght spot

drivetrain/controlStrategies/holonomicDriveController.py

@@ -43,12 +43,12 @@ def __init__(self, name:str):
         self.yFB = 0.0
         self.tFB = 0.0
 
-        #addLog(f"{name} HDC xFF", lambda:self.xFF, "mps")
-        #addLog(f"{name} HDC yFF", lambda:self.yFF, "mps")
-        #addLog(f"{name} HDC tFF", lambda:self.tFF, "radpersec")
-        #addLog(f"{name} HDC xFB", lambda:self.xFB, "mps")
-        #addLog(f"{name} HDC yFB", lambda:self.yFB, "mps")
-        #addLog(f"{name} HDC tFB", lambda:self.tFB, "radpersec")
+        addLog(f"{name} HDC xFF", lambda:self.xFF, "mps")
+        addLog(f"{name} HDC yFF", lambda:self.yFF, "mps")
+        addLog(f"{name} HDC tFF", lambda:self.tFF, "radpersec")
+        addLog(f"{name} HDC xFB", lambda:self.xFB, "mps")
+        addLog(f"{name} HDC yFB", lambda:self.yFB, "mps")
+        addLog(f"{name} HDC tFB", lambda:self.tFB, "radpersec")
 
         # Closed-loop control for the X position
         self.xCtrl = PIDController(

drivetrain/drivetrainPhysical.py

@@ -142,7 +142,7 @@ def dtMotorRotToLinear(rot):
     Translation3d(
         inchesToMeters(14.5), inchesToMeters(2.75), inchesToMeters(8)  # X  # Y  # Z
     ),
-    Rotation3d(0.0, 0.0, 0.0),  # Roll  # Pitch  # Yaw
+    Rotation3d.fromDegrees(0.0, 3.0, 0.0),  # Roll  # Pitch  # Yaw
 )
 
 

navigation/navForce.py

@@ -11,7 +11,10 @@ def logisticFunc(x, L, k, x0):
     https://en.wikipedia.org/wiki/Logistic_function
     This function is nice due to being exactly 0 at one side, 1.0 at the other, and a smooth transition between the two
     """
-    return L / (1 + math.exp(-k * (x - x0)))
+    try:
+        return L / (1 + math.exp(-k * (x - x0)))
+    except OverflowError:
+        return 0.0
 
 @dataclass
 class Force:

robot.py

@@ -71,8 +71,7 @@ def robotPeriodic(self):
 
         self.autodrive.updateTelemetry()
         self.driveTrain.poseEst._telemetry.setCurAutoDriveWaypoints(self.autodrive.getWaypoints())
-        #self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.getObstacles())
-        self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths())
+        #self.driveTrain.poseEst._telemetry.setCurObstacles(self.autodrive.rfp.getObstacleStrengths())
         self.stt.mark("Telemetry")
 
         self.ledCtrl.setAutoDrive(self.autodrive.isRunning())

utils/rioMonitor.py

@@ -42,7 +42,7 @@ def __init__(self):
         self.intDiskUsage = 0
         self.extDiskUsage = 0
 
-        addLog("RIO Supply Voltage", RobotController.getInputVoltage, "V")
+        #addLog("RIO Supply Voltage", RobotController.getInputVoltage, "V")
         #addLog("RIO CAN Bus Usage", lambda: self.CANBusUsage, "pct")
         #addLog(
         #    "RIO CAN Bus Err Count",

wrappers/wrapperedObstaclePhotonCamera.py

@@ -25,7 +25,7 @@ def _calculateDistanceToTargetMeters(
     using right triangles, the assumption obstacles are on the ground, and the geometry
     of where the camera is mounted on the robot.
     """
-    netAngle = cameraPitchRadians + targetPitchRadians
+    netAngle = -1.0 * cameraPitchRadians + targetPitchRadians
 
     # Even if there's some error in measurement, be sure the angle puts the target 
     # in the right place.
@@ -91,7 +91,7 @@ def update(self):
         for target in res.getTargets():
             # Transform both poses to on-field poses
             if (target.getArea()>MIN_AREA):
-                print(f"Saw target at pitch {target.getPitch()} deg, yaw {target.getYaw()} deg")
+                #print(f"Saw target at pitch {target.getPitch()} deg, yaw {target.getYaw()} deg")
                 # Use algorithm described at 
                 # https://docs.limelightvision.io/docs/docs-limelight/tutorials/tutorial-estimating-distance
                 # to estimate distance from the camera to the target.
@@ -106,7 +106,7 @@ def update(self):
                     Rotation2d.fromDegrees(target.getYaw())
                 )
                 camToObstacleTransform = Transform2d(camToObstacleTranslation, Rotation2d())
-                print(f"Saw big target at {camToObstacleTransform}")
+                #print(f"Saw big target at {camToObstacleTransform}")
 
                 self.obstacleEstimates.append(camToObstacleTransform)
 

wrappers/wrapperedPulseWidthEncoder.py

@@ -43,7 +43,7 @@ def __init__(
 
         #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")
+        #addLog(f"{self.name}_angle", lambda: self.curAngleRad, "rad")
 
     def update(self):
         """Return the raw angle reading from the sensor in radians"""

wrappers/wrapperedSparkMax.py

@@ -74,13 +74,13 @@ def __init__(self, canID, name, brakeMode=False, currentLimitA=40.0):
 
         self.disconFault.set(not self.configSuccess)
 
-        addLog(self.name + "_outputCurrent", self.ctrl.getOutputCurrent, "A")
+        #addLog(self.name + "_outputCurrent", self.ctrl.getOutputCurrent, "A")
         #addLog(self.name + "_appliedOutput", self.ctrl.getAppliedOutput, "%")
-        addLog(self.name + "_desVolt", lambda: self.desVolt, "V")
+        #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")
+        #addLog(self.name + "_actPos", lambda: self.actPos, "rad")
+        #addLog(self.name + "_actVel", lambda: self.actVel, "RPM")
 
 
     def setInverted(self, isInverted):