Update codes_for_scrimmage/hazmat/mainhazmatUPD.py

codes_for_scrimmage/hazmat/mainhazmatUPD.py

@@ -7,10 +7,8 @@ from pybricks.hubs import PrimeHub
 
 # Initialize hub and devices
 hub = PrimeHub()
-
 left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
 right_motor = Motor(Port.B)
-
 left_arm = Motor(Port.C)
 right_arm = Motor(Port.D)
 lazer_ranger = UltrasonicSensor(Port.E)
@@ -23,22 +21,22 @@ drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, 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."""
-    #await drive_base.straight(5000)
     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()
+        distance = lazer_ranger.distance()
         
         if distance < WALL_DISTANCE:
             # Stop the drivebase
             await drive_base.turn(-180)
-            drive_base.brake
+            drive_base.brake()
             print(f"Wall detected at {distance}mm!")
             break
         
@@ -50,18 +48,15 @@ async def Run1(): #From M8_5.py
     left_arm.run_angle(1000, 300)
     right_arm.run_angle(1000, 500)
     await drive_base.straight(320)
-
     await right_arm.run_angle(5000, -500, Stop.HOLD)
     await right_arm.run_angle(5000, 500, Stop.HOLD)
     await right_arm.run_angle(5000, -500, Stop.HOLD)
     await right_arm.run_angle(5000, 500, Stop.HOLD)
     await right_arm.run_angle(5000, -500, Stop.HOLD)
-
     await drive_base.turn(-20)
     await drive_base.straight(277)
     await drive_base.turn(20)
     await drive_base.straight(65)
-
     await drive_base.turn(-30)
     right_arm.run_angle(50, 500)
     await drive_base.turn(45)
@@ -81,12 +76,10 @@ async def Run2(): #From Heavy_lifting_final.py
     await drive_base.straight(536)
     await drive_base.turn(60, Stop.HOLD)
     await drive_base.straight(30)
-
     await right_arm.run_angle(5000, 2900)
     await drive_base.straight(40)
     await right_arm.run_angle(5000, -4000)
     await drive_base.straight(-60)
-
     await drive_base.turn(-60)
     await drive_base.straight(-670)
 
@@ -113,11 +106,10 @@ async def Run3(): #tip the scale.py
     await drive_base.turn(46.5)
     await drive_base.turn(-40)
     await drive_base.straight(900)
-
     drive_base.settings(600, 500, 300, 200)  # Reset it to the initial values
 
-async def Run4(): #From Send_Over_Final.py
 
+async def Run4():  # From Send_Over_Final.py
     # Get to mission
     await drive_base.straight(920)
     await drive_base.turn(-90, Stop.HOLD)
@@ -130,22 +122,12 @@ async def Run4(): #From Send_Over_Final.py
     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()
     )
 
+
 # Add Rishi's code here
 async def Run5(): 
     await drive_base.straight(519)
@@ -174,6 +156,7 @@ async def Run5():
     await right_arm.run_angle(3000, 3000)
     await drive_base.turn(-40)
 
+
 # Add - Adi's code here
 async def Run6():
     await drive_base.straight(420)
@@ -182,40 +165,53 @@ async def Run6():
     await right_arm.run_angle(300, 100)
     await drive_base.straight(-350)
 
+
 # Function to classify color based on HSV
-async def main:
+def detect_color(h, s, v, reflected):
     if reflected > 4:
         if h < 4 or h > 350:  # red
             return "Red"
-            print('Running Mission 3')
-            await Run3()
         elif 3 < h < 40 and s > 70:  # orange
             return "Orange"
-            print('Running Mission 6')
-            await Run6()
         elif 47 < h < 56:  # yellow
             return "Yellow"
-            print('Running Mission 4')
-            await Run4()
         elif 70 < h < 160:  # green - do it vertically not horizontally for accuracy
             return "Green"
-            print('Running Mission 1')
-            await Run1()
         elif 210 < h < 225:  # blue - do it vertically not horizontally for accuracy
             return "Blue"
-            print('Running Mission 5')
+        elif 260 < h < 350:  # purple
-            await Run5()
-        elif 230 < h < 320:   #purple
             return "Purple"
-            print('Running Mission 2')
-            await Run2()
         else:
             return "Unknown"
+    return "Unknown"
 
-# Main loop
+
+async def main():
     while True:
-    h, s, v = sensor.hsv()
+        h, s, v = await color_sensor.hsv()
-    reflected = sensor.reflection()
+        reflected = await color_sensor.reflection()
         color = detect_color(h, s, v, reflected)
-    print(f"Detected Color: {color} (Hue: {h}, Sat: {s}, Val: {v})")
+        
-    wait(1000)
+        if color == "Red":
+            print('Running Mission 3')
+            await Run3() #red
+        elif color == "Orange":
+            print('Running Mission 6')
+            await Run6() #orange
+        elif color == "Yellow":
+            print('Running Mission 4')
+            await Run4() #yellow
+        elif color == "Green": 
+            print('Running Mission 1')
+            await Run1() #green - vertically
+        elif color == "Blue":
+            print('Running Mission 5')
+            await Run5() #blue - vertically
+        elif color == "Purple":
+            print('Running Mission 2')
+            await Run2()  #purple - vertically
+        else:
+            print(f"Unknown color detected (Hue: {h}, Sat: {s}, Val: {v})")
+        await wait(10)
+# Run the main function
+run_task(main())