Merge branch 'main_nav' into KylesStuckDetector

# Conflicts:
#	drivetrain/drivetrainCommand.py
#	navigation/repulsorFieldPlanner.py

Autonomous/commands/driveForwardSlowCommand.py

@@ -26,4 +26,4 @@ def isDone(self):
         return Timer.getFPGATimestamp() - self.startTime >= 3
 
     def end(self,interrupt):
-        return self.returnDriveTrainCommand
+        pass

drivetrain/controlStrategies/autoDrive.py

@@ -90,18 +90,18 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
 
             # 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):
-                self._olCmd = self.rfp.update(curPose, MAX_PATHPLAN_SPEED_MPS*0.02)
+            if(self._prevCmd is None):
+                initCmd = DrivetrainCommand(0,0,0,curPose) # TODO - init this from current odometry vel
+                self._olCmd = self.rfp.update(initCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
             else:
-                self._olCmd = self.rfp.update(self._prevCmd.desPose, MAX_PATHPLAN_SPEED_MPS*0.02)
+                self._olCmd = self.rfp.update(self._prevCmd, 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)    
+            retCmd = self._trajCtrl.update2(self._olCmd.velX, 
+                                            self._olCmd.velY, 
+                                            self._olCmd.velT, 
+                                            self._olCmd.desPose, curPose)    
             self._prevCmd = retCmd
         else:
             self._prevCmd = None

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/drivetrainCommand.py

@@ -1,5 +1,5 @@
 from wpimath.geometry import Pose2d
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 
 
 
@@ -10,6 +10,6 @@ class DrivetrainCommand:
     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
+    velY:float = 0.0  # Field Y velocity in meters/secS
     velT:float = 0.0  # Rotational speed in rad/sec
-    desPose: Pose2d | None = None  # Current desired pose of the drivetrain, or None if no pose is specified.
+    desPose:Pose2d = field(default_factory = lambda: Pose2d())  # Current desired pose of the drivetrain

drivetrain/drivetrainControl.py

@@ -93,11 +93,7 @@ def update(self):
         self.desChSpd = _discretizeChSpd(tmp)
 
         # Set the desired pose for telemetry purposes
-        if self.curCmd.desPose is not None:
-            desPose = self.curCmd.desPose
-        else:
-            desPose = curEstPose
-        self.poseEst._telemetry.setDesiredPose(desPose)
+        self.poseEst._telemetry.setDesiredPose(self.curCmd.desPose)
 
         # Given the current desired chassis speeds, convert to module states
         desModStates = kinematics.toSwerveModuleStates(self.desChSpd)

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
 )
 
 

drivetrain/poseEstimation/drivetrainPoseTelemetry.py

@@ -21,7 +21,9 @@ def __init__(self):
         wpilib.SmartDashboard.putData("DT Pose 2D", self.field)
         self.curTraj = Trajectory()
         self.curTrajWaypoints = []
-        self.obstacles = []
+        self.fullObstacles = []
+        self.thirdObstacles = []
+        self.almostGoneObstacles = []
 
         self.desPose = Pose2d()
 
@@ -55,8 +57,8 @@ def addVisionObservations(self, observations:list[CameraPoseObservation]):
             for obs in observations:
                 self.visionPoses.append(obs.estFieldPose)
 
-    def setCurObstacles(self, obstacles:list[Translation2d]):
-        self.obstacles = obstacles
+    def setCurObstacles(self, obstacles):
+        self.fullObstacles, self.thirdObstacles, self.almostGoneObstacles = obstacles
 
     def clearVisionObservations(self):
         self.visionPoses = []
@@ -75,7 +77,13 @@ def update(self, estPose:Pose2d, moduleAngles):
         self.field.getObject("desPose").setPose(self.desPose)
         self.field.getObject("desTraj").setTrajectory(self.curTraj)
         self.field.getObject("desTrajWaypoints").setPoses(self.curTrajWaypoints)
-        self.field.getObject("curObstacles").setPoses([Pose2d(x, Rotation2d()) for x in self.obstacles])
+        self.field.getObject("curObstaclesFull").setPoses([Pose2d(x, Rotation2d()) for x in self.fullObstacles])
+        self.field.getObject("curObstaclesThird").setPoses([Pose2d(x, Rotation2d()) for x in self.thirdObstacles])
+        self.field.getObject("curObstaclesAlmostGone").setPoses([Pose2d(x, Rotation2d()) for x in self.almostGoneObstacles])
+        #print("Full: " + str(len(self.fullObstacles)))
+        #print("Third: " + str(len(self.thirdObstacles)))
+        #print("Almost gone: " + str(len(self.almostGoneObstacles)))
+
 
         self.field.getObject("visionObservations").setPoses(self.visionPoses)
         self.visionPoses = []

glass-window.json

@@ -0,0 +1,51 @@
+{
+  "Docking": {
+    "Data": [
+      "DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=0,19 Size=1920,990 CentralNode=1"
+    ]
+  },
+  "MainWindow": {
+    "GLOBAL": {
+      "fps": "120",
+      "height": "1009",
+      "maximized": "1",
+      "style": "0",
+      "userScale": "2",
+      "width": "1920",
+      "xpos": "0",
+      "ypos": "23"
+    }
+  },
+  "Window": {
+    "###/SmartDashboard/DT Pose 2D": {
+      "Collapsed": "0",
+      "Pos": "456,119",
+      "Size": "1338,714"
+    },
+    "###NetworkTables": {
+      "Collapsed": "0",
+      "Pos": "250,277",
+      "Size": "750,723"
+    },
+    "###NetworkTables Info": {
+      "Collapsed": "0",
+      "Pos": "251,37",
+      "Size": "750,158"
+    },
+    "###NetworkTables Settings": {
+      "Collapsed": "0",
+      "Pos": "30,30",
+      "Size": "329,169"
+    },
+    "Debug##Default": {
+      "Collapsed": "0",
+      "Pos": "60,60",
+      "Size": "400,400"
+    },
+    "DockSpaceViewport_11111111": {
+      "Collapsed": "0",
+      "Pos": "0,19",
+      "Size": "1920,990"
+    }
+  }
+}

glass.json

@@ -0,0 +1,98 @@
+{
+  "NetworkTables": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/DT Pose 2D": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/DT Pose 2D": {
+        "ModulePoses": {
+          "color": [
+            255.0,
+            157.5,
+            0.0,
+            255.0
+          ],
+          "image": "C:\\Users\\chris\\git\\RobotCasseroleSwerveBase2025\\swerve_module.png",
+          "length": 0.4000000059604645,
+          "weight": 2.0,
+          "width": 0.4000000059604645
+        },
+        "Robot": {
+          "color": [
+            255.0,
+            202.50001525878906,
+            0.0,
+            255.0
+          ]
+        },
+        "curObstacles": {
+          "image": "C:\\Users\\chris\\git\\RobotCasseroleSwerveBase2025\\obstacle.png",
+          "length": 0.6000000238418579,
+          "width": 0.6000000238418579
+        },
+        "desPose": {
+          "arrows": false
+        },
+        "height": 8.013679504394531,
+        "visionObservations": {
+          "arrowWeight": 1.0,
+          "arrows": false,
+          "color": [
+            0.22049212455749512,
+            0.6078431606292725,
+            0.58505779504776,
+            255.0
+          ],
+          "weight": 6.0
+        },
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "JSClient_33109861@3": {
+      "Publishers": {
+        "open": false
+      },
+      "Subscribers": {
+        "open": false
+      },
+      "open": false
+    },
+    "JSClient_52050899@2": {
+      "Publishers": {
+        "open": false
+      },
+      "Subscribers": {
+        "open": false
+      },
+      "open": false
+    },
+    "JSClient_75908461@3": {
+      "Subscribers": {
+        "open": false
+      },
+      "open": false
+    },
+    "glass@3": {
+      "open": false
+    },
+    "photonvision@1": {
+      "Publishers": {
+        "open": false
+      }
+    },
+    "photonvision@2": {
+      "open": false
+    },
+    "visible": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)",
+    "serverTeam": "1736"
+  }
+}

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:

navigation/obstacleDetector.py

@@ -1,7 +1,7 @@
 
 from wrappers.wrapperedObstaclePhotonCamera import WrapperedObstaclePhotonCamera
 from drivetrain.drivetrainPhysical import ROBOT_TO_FRONT_CAM
-from wpimath.geometry import Pose2d
+from wpimath.geometry import Pose2d, Pose3d
 from navigation.forceGenerators import ForceGenerator, PointObstacle
 
 class ObstacleDetector():
@@ -19,8 +19,9 @@ def getObstacles(self, curPose:Pose2d) -> list['ForceGenerator']:
 
         self.frontCam.update()
         for obs in self.frontCam.getObstacles():
-            obsPose = curPose.translation() + obs
-            retList.append(PointObstacle(location=obsPose, strength=0.7))
+            camPose = Pose3d(curPose).transformBy(ROBOT_TO_FRONT_CAM).toPose2d()
+            obsTrans = camPose.transformBy(obs)
+            retList.append(PointObstacle(location=obsTrans.translation(), strength=0.7))
 
         # TODO - add other cameras and their observations