Update missions/Send_Over_Final.py

missions/Send_Over_Final.py

@@ -19,28 +19,41 @@ drive_base = DriveBase(left_motor, right_motor, wheel_diameter=68.8, axle_track=
 drive_base.settings(600,500,300,200)
 drive_base.use_gyro(True)
 
+WALL_DISTANCE = 200  # mm
+
+async def drive_forward():
+    """Drive forward continuously using DriveBase."""
+    drive_base.drive(400, 0)
+
+
+async def monitor_distance():
+    """Monitor ultrasonic sensor and stop when wall is detected."""
+    while True:
+        distance = await lazer_ranger.distance()
+        print('Distancing...',distance)
+        
+        if distance < WALL_DISTANCE:
+            # Stop the drivebase
+            await drive_base.stop
+            print(f"Wall detected at {distance}mm!")
+            break
+        
+        # Small delay to prevent overwhelming the sensor
+        await wait(50)
+
 async def main():
-    #Get to mission
-    await drive_base.straight(920)
+    await drive_base.straight(-920)
     await drive_base.turn(-90,Stop.HOLD)
     await drive_base.straight(65)
-    #Solve mission
+    #Solve
     drive_base.turn(-10)
     await left_arm.run_angle(10000,-4000)
-    #Get to Red Start
     await drive_base.straight(-110)
     await drive_base.turn(90)
-    await drive_base.straight(500)
-    while True:
-        distance_mm = await lazer_ranger.distance()
-        print('distancing...',distance_mm)
-
-        if distance_mm < 300:
-            drive_base.stop
-            break
-        else:
-            drive_base.straight(300)
-            print('running...')
-        await wait(10)
 
+await multitask(
+        drive_forward(), 
+        monitor_distance()
+    )
+    
 run_task(main())