Create TheMazeRunner.py

BothellRobotics · web-flow · commit 9864555c201f · 2019-06-01T08:37:06.000-07:00
diff --git a/TheMazeRunner.py b/TheMazeRunner.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env/ python3
+
+from ev3dev2.sensor.lego import UltrasonicSensor
+from ev3dev2.motor import MoveTank, OUTPUT_A, OUTPUT_D
+from ev3dev2.button import Button
+import time
+from ev3dev2.sensor.lego import ColorSensor
+import logging
+
+logging.basicConfig(level=logging.DEBUG,
+        format='%(asctime)s %(levelname)5s: %(message)s')
+log = logging.getLogger(__name__)
+
+cs = ColorSensor()
+us = UltrasonicSensor()
+tp = MoveTank(OUTPUT_A, OUTPUT_D)
+button = Button()
+
+try:
+    while not button.any():
+        
+            #white line and turn
+        tp.on(left_speed = 50, right_speed = 50)
+        time.sleep(1.5)
+        while not button.any():
+            CS = cs.reflected_light_intensity 
+            log.info(CS)
+            if CS > 15:
+                break
+        tp.on_for_rotations(-50, 50, 0.65)
+            
+            #yellow line and turn
+        tp.on(left_speed = 50, right_speed = 50)
+        time.sleep(1.5)
+        while not button.any():    
+            if cs.reflected_light_intensity > 15:
+                break
+        tp.on_for_rotations(50, -50, 0.65)
+
+            #Cabinet obstruction
+        while not button.any():    
+            tp.on(left_speed = 50, right_speed = 50)
+            dist = int( us.distance_centimeters_continuous)
+            log.info(dist)
+            if dist < 33:
+                break
+        tp.on_for_rotations(-50, 50, 0.65)
+
+            #Finish line      
+        tp.on(left_speed = 50, right_speed = 50)
+        time.sleep(1)
+        while not button.any():    
+            if cs.reflected_light_intensity > 15:
+                break
+        
+        tp.on(left_speed = 0, right_speed = 0)
+        log.info("Done")
+        break
+    log.info("Complete")
+
+
+except(KeyboardInterrupt):
+    print('interrupt')
+    tp.on(left_speed = 0, right_speed = 0)
+        
+
