backup solution

shittyrobot.py

@@ -59,6 +59,7 @@ def setup_listener():
     s.bind(addr)
     s.listen(3)
     #s.setblocking(False)
+    s.settimeout(0.1)
     return s
 
 
@@ -68,7 +69,7 @@ MOTOR_B0 = Pin(2, Pin.OUT, drive=Pin.DRIVE_0)
 MOTOR_B1 = Pin(3, Pin.OUT, drive=Pin.DRIVE_0)
 #LED_DAT = Pin(6, Pin.OUT, drive=Pin.DRIVE_3)
 #LED_NP = neopixel.NeoPixel(LED_DAT, 6, timing=1, bpp=4)
-
+BTN = Pin(9, Pin.IN, Pin.PULL_UP)
 
 #do_connect()
 do_ap()
@@ -77,7 +78,9 @@ c = []
 
 debug = False
 
-def controlloop(last_start, next_action):
+#global min_time = 601000
+
+def controlloop(last_start, next_action, min_time):
     now = time.ticks_us()
 
     if debug:
@@ -88,12 +91,19 @@ def controlloop(last_start, next_action):
     #        print(' DEADTIME -> ignore')
     #    return last_start
 
-    if next_action is None and time.ticks_diff(time.ticks_add(last_start, 601000), now) > 0:
+    #min_time = 601000
+    if (next_action == (1,1)) or (next_action == (-1,-1)):
+        #min_time = 991000
+        min_time = 2222000
+    elif (next_action == (1,-1)) or (next_action == (-1,1)):
+        min_time = 601000
+    
+    if next_action is None and time.ticks_diff(time.ticks_add(last_start, min_time), now) > 0:
         if debug:
             print(' ACTION TIMED OUT -> stopping')
         next_action = (0,0)
     elif next_action is None:
-        return last_start
+        return (last_start, min_time)
 
     if debug:
         print('Starting action: ', next_action)
@@ -120,14 +130,43 @@ def controlloop(last_start, next_action):
 
     # we just started new action
     if next_action != (0,0):
-        return time.ticks_us()
+        return (time.ticks_us(), min_time)
     else:
-        return last_start
+        return (last_start, min_time)
+
+
+def backup_solution():
+    time.sleep(1)
+    MOTOR_A0.on()
+    MOTOR_A1.off()
+    MOTOR_B0.off()
+    MOTOR_B1.on()
+    time.sleep(10)
+    MOTOR_B0.off()
+    MOTOR_B1.off()
+    MOTOR_A0.off()
+    MOTOR_A1.off()
+
+
+def check_backup_solution(iters=10):
+    for i in range(iters):
+        if BTN.value() == 0:
+            backup_solution()
+            return
+        time.sleep_ms(10)
         
     
 def mainloop(l):
     print('Accepting connection')
+    y = None
+    while True:
+        try:
             (y, origin_sockaddr) = s.accept()
+            break
+        except OSError:
+            check_backup_solution()
+            continue
+    
     y.settimeout(0.1)
     y.write("\255\254\34Welcome!\nPress wasd to move!\n>")
 
@@ -143,10 +182,11 @@ def mainloop(l):
     #LED_NP.write()
     #time.sleep(1)
 
+    min_time = 601000
     while True:
         ctr += 1
         # run one iteration of control loop
-        last_action_start = controlloop(last_action_start, next_action)
+        (last_action_start, min_time) = controlloop(last_action_start, next_action, min_time)
         #if time.time() > last_pixel_update + 0.5:
         #    #LED_NP[ctr % 6] = (ctr*3 % 255, ctr*2 % 255, (ctr+1)*3 % 255)
         #    last_pixel_update = time.time()
@@ -157,7 +197,7 @@ def mainloop(l):
             a = y.read(1)
             if debug:
                 print('RCV: ', a)
-            y.write(a)
+            y.write(str(a).upper())
         except OSError as e:
             if e.errno == errno.ETIMEDOUT or e.errno == errno.EAGAIN:
                 # timeout
@@ -168,6 +208,7 @@ def mainloop(l):
                     print('Socket error: ', e)
                 y.close()
                 mainloop(l)
+        check_backup_solution(3)
         if a == b'':
             # EOF
             y.close()