Fix initial depth issues with gamepad sassist

MB3hel · Sep 18, 2023 · b816c50 · b816c50
1 parent b97904d
commit b816c50
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/iface/example/gamepad_sassist.py b/iface/example/gamepad_sassist.py
@@ -45,7 +45,7 @@ def run(cb: ControlBoard, s: Simulator) -> int:
     time.sleep(0.5) # Wait for data to actually be sent
 
     print("Enter main loop")
-    depth_target = cb.get_ms5837_data().depth
+    depth_target = -0.2
     pitch_target = 0.0
     roll_target = 0.0
     depth_ramp = 0.0
@@ -63,7 +63,7 @@ def run(cb: ControlBoard, s: Simulator) -> int:
             dpad = gp0.get_dpad(0)
 
             if dpad == 8 or dpad == 1 or dpad == 2:
-                depth_target = cb.get_ms5837_data().depth + (0.2 + depth_ramp)
+                depth_target = cb.get_ms5837_data().depth + (0.05)
                 depth_ramp += 0.01
             elif dpad == 6 or dpad == 5 or dpad == 4:
                 depth_target = cb.get_ms5837_data().depth - (0.2 + depth_ramp)
@@ -74,6 +74,9 @@ def run(cb: ControlBoard, s: Simulator) -> int:
             if depth_ramp > 0.3:
                 depth_ramp = 0.3
 
+            if depth_target > -0.1:
+                depth_target = -0.1
+
             # Scale speeds to make the robot more controllable
             x *= 0.5
             y *= -0.5