Merge pull request #296 from thedropbears/make_reef_intake_safer

limit safing to longitudinal velocity

Author: LucienMorey

components/chassis.py

@@ -381,6 +381,10 @@ def drive_local(self, vx: float, vy: float, omega: float) -> None:
         """Robot oriented drive commands"""
         self.chassis_speeds = ChassisSpeeds(vx, vy, omega)
 
+    def limit_to_longitudinal_velocity(self) -> None:
+        self.chassis_speeds.vy = 0.0
+        self.chassis_speeds.omega = 0.0
+
     def snap_to_heading(self, heading: float) -> None:
         """set a heading target for the heading controller"""
         self.snapping_to_heading = True

controllers/reef_intake.py

@@ -99,6 +99,9 @@ def safing(self, initial_call: bool):
             robot_pose.translation()
         )
 
+        if not wpilib.DriverStation.isAutonomous() and distance < self.RETREAT_DISTANCE:
+            self.chassis.limit_to_longitudinal_velocity()
+
         if (
             distance >= self.RETREAT_DISTANCE
             and not self.algae_measurement.is_executing