Update algorithm

folkrace.py

@@ -15,74 +15,112 @@ front = adafruit_hcsr04.HCSR04(trigger_pin=board.D12, echo_pin=board.D13)
 right = adafruit_hcsr04.HCSR04(trigger_pin=board.D10, echo_pin=board.D11)
 left = adafruit_hcsr04.HCSR04(trigger_pin=board.D6, echo_pin=board.D9)
 
+speed = 0.6
+
 def forward():
-    m1.throttle = 0.5
-    m2.throttle = 0.5
-    m3.throttle = 0.5
-    m4.throttle = 0.5
+    m1.throttle = speed
+    m2.throttle = speed
+    m3.throttle = speed
+    m4.throttle = speed
 
 def backward():
-    m1.throttle = -0.5
-    m2.throttle = -0.5
-    m3.throttle = -0.5
-    m4.throttle = -0.5
+    m1.throttle = -speed
+    m2.throttle = -speed
+    m3.throttle = -speed
+    m4.throttle = -speed
 
 def stleft():
-    m1.throttle = -0.5
-    m2.throttle = 0.5
-    m3.throttle = 0.5
-    m4.throttle = -0.5
-    
+    m1.throttle = -speed
+    m2.throttle = speed
+    m3.throttle = speed
+    m4.throttle = -speed
+
 def stright():
-    m1.throttle = 0.5
-    m2.throttle = -0.5
-    m3.throttle = -0.5
-    m4.throttle = 0.5
+    m1.throttle = speed
+    m2.throttle = -speed
+    m3.throttle = -speed
+    m4.throttle = speed
 
 def dgright():
-    m1.throttle =0.5
+    m1.throttle =speed
     m2.throttle = 0
     m3.throttle = 0
-    m4.throttle =0.5
+    m4.throttle =speed
 
 def dgleft():
     m1.throttle = 0
-    m2.throttle =0.5
-    m3.throttle =0.5
+    m2.throttle =speed
+    m3.throttle =speed
     m4.throttle = 0
-    
+
 def turnright():
-    m1.throttle = 0.5
-    m2.throttle = 0.5
-    m3.throttle = 0.5
+    m1.throttle = speed
+    m2.throttle = speed
+    m3.throttle = speed
     m4.throttle = 0
-    
+
 def turnleft():
-    m1.throttle = 0.5
-    m2.throttle = 0.5
+    m1.throttle = speed
+    m2.throttle = speed
     m3.throttle = 0
-    m4.throttle = 0.5
+    m4.throttle = speed
+
+def rotright():
+    m1.throttle = speed
+    m2.throttle = - speed
+    m3.throttle = 0
+    m4.throttle = 0
+
+def rotleft():
+    m1.throttle = - speed
+    m2.throttle = speed
+    m3.throttle = 0
+    m4.throttle = 0
 
 while True:
     print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}")
     try:
-        if front.distance <= 20: # What to do when there is an obstacle ahead
+        if front.distance <= 35: # What to do when there is an obstacle ahead
             if left.distance > right.distance: # Diagonally left
                 dgleft()
                 time.sleep(0.8)
             if right.distance > left.distance: # Diagonally right
                 dgright()
                 time.sleep(0.8)
-        if left.distance < 30: # Strafe right
+
+        if front.distance < 10:
+            backward()
+            time.sleep(0.7)
+            if left.distance > right.distance: # Diagonally left
+                rotleft()
+                time.sleep(0.5)
+            if right.distance > left.distance: # Diagonally right
+                rotright()
+                time.sleep(0.5)
+
+        if left.distance < 50: # Strafe right
             turnright()
-            #stright()
-            time.sleep(0.5)
-        if right.distance < 30: # Strafe left
+            time.sleep(0.4)
+        if right.distance < 50: # Strafe left
             turnleft()
-            #stleft()
+            time.sleep(0.4)
+
+        if left.distance < 15: # Strafe right
+            #turnright()
+            dgright()
             time.sleep(0.5)
+            rotright()
+            time.sleep(0.4)
+        if right.distance < 15: # Strafe left
+            #turnleft()
+            dgleft()
+            time.sleep(0.5)
+            rotleft()
+            time.sleep(0.4)
+
+
         else:
             forward()
 
     except RuntimeError:
-        pass
+        pass