adding recent scripts: reset, state, watchdog, etc

apriltags.py

@@ -19,9 +19,9 @@ def degrees(radians):
 y_rot = 0
 z_rot = 0
 while(True):
-    img = sensor.snapshot().rotation_corr(x_rotation = x_rot, \
-                                          y_rotation = y_rot, \
-                                          z_rotation = z_rot, \
+    img = sensor.snapshot().rotation_corr(x_rotation = 180, \
+                                          y_rotation = 360, \
+                                          z_rotation = 0, \
                                           x_translation = 0, \
                                           y_translation = 0, \
                                           zoom = 1)
@@ -32,6 +32,6 @@ def degrees(radians):
             degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
 
         x_rot += 180 - degrees(tag.x_rotation())
-        y_rot += -degrees(tag.y_rotation())
-        z_rot += -degrees(tag.z_rotation())
+        y_rot -= degrees(tag.y_rotation())
+        z_rot += degrees(tag.z_rotation())
         print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)

donkey.py

@@ -14,10 +14,13 @@
 REVERSE = False
 COLOR_LINE_FOLLOWING = True # False to use grayscale thresholds, true to use color thresholds.
 JUMP_START_THROTTLE_INCREASE = 1
-DEBUG_PRINT_UART = True # whether to write UART debug
+DEBUG_PRINT_UART = False # whether to write UART debug
 DEBUG_LINE_STATUS = True
-#COLOR_THRESHOLDS = [( 85, 100,  -40,  127,   20,  127)] # Yellow Line.
+#COLOR_THRESHOLDS = [( 85, 100, -40, 127, 20, 127)] # Yellow Line.
+#COLOR_THRESHOLDS =[(85, 100, -3, 127, -9, 94)] # do space
 COLOR_THRESHOLDS = [(1, 80, -25, 28, -58, -10)] # Blue tape line
+#kitchen table:
+#COLOR_THRESHOLDS = [(30, 78, -18, 28, -70, -19)]
 GRAYSCALE_THRESHOLDS = [(240, 255)] # White Line.
 BINARY_VIEW = False # Helps debugging but costs FPS if on.
 DO_NOTHING = False # Just capture frames...
@@ -29,13 +32,13 @@
 AREA_THRESHOLD = 20 # Raise to filter out false detections.
 PIXELS_THRESHOLD = 20 # Raise to filter out false detections.
 MAG_THRESHOLD = 5 # Raise to filter out false detections.
-MIXING_RATE = 0.6 # Percentage of a new line detection to mix into current steering.
+MIXING_RATE = 0.8 # Percentage of a new line detection to mix into current steering.
 
 # Tweak these values for your robocar.
 THROTTLE_CUT_OFF_ANGLE = 2.0 # Maximum angular distance from 90 before we cut speed [0.0-90.0).
 THROTTLE_CUT_OFF_RATE = 10.0 # How much to cut our speed boost (below) once the above is passed (0.0-1.0].
 THROTTLE_GAIN = 10.0 # e.g. how much to speed up on a straight away
-THROTTLE_OFFSET = 13 # e.g. default speed (0 to 100)
+THROTTLE_OFFSET = 30 # e.g. default speed (0 to 100)
 THROTTLE_P_GAIN = 1.0
 THROTTLE_I_GAIN = 0.0
 THROTTLE_I_MIN = -0.0
@@ -44,23 +47,23 @@
 MIN_THROTTLE = 10
 
 # Tweak these values for your robocar.
-STEERING_OFFSET = 90 # Change this if you need to fix an imbalance in your car (0 to 180).
+STEERING_FACTOR = 1.2 # 1-
+STEERING_OFFSET = 100 # Change this if you need to fix an imbalance in your car (0 to 180).
 STEERING_P_GAIN = -40.0 # Make this smaller as you increase your speed and vice versa.
 STEERING_I_GAIN = 0.0
-STEERING_I_MIN = -0.0
+STEERING_I_MIN = 0.0
 STEERING_I_MAX = 0.0
 STEERING_D_GAIN = -9 # Make this larger as you increase your speed and vice versa.
 
 # Tweak these values for your robocar.
-THROTTLE_SERVO_MIN_US = 1550
-THROTTLE_SERVO_MAX_US = 1700
+#THROTTLE_SERVO_MIN_US = 1550
+#THROTTLE_SERVO_MAX_US = 2100
+THROTTLE_SERVO_MIN_US = 1500
+THROTTLE_SERVO_MAX_US = 2000
 
 # Tweak these values for your robocar.
-STEERING_SERVO_MIN_US = 700
-STEERING_SERVO_MAX_US = 2300
-#STEERING_SERVO_MIN_US = 1300
-## back/right max
-#STEERING_SERVO_MAX_US = 1800
+STEERING_SERVO_MIN_US = 1276
+STEERING_SERVO_MAX_US = 1884
 
 FRAME_REGION = max(min(FRAME_REGION, 1.0), 0.0)
 FRAME_WIDE = max(min(FRAME_WIDE, 1.0), 0.0)
@@ -96,11 +99,17 @@ def figure_out_my_steering(line, img):
     # of the image and to the left or right such that the line goes through it (cx may be off the image).
     cy = img.height() / 2
     cx = (line.rho() - (cy * math.sin(math.radians(line.theta())))) / math.cos(math.radians(line.theta()))
+    #cx2 = (line.rho() - (cy * math.sin(math.radians(line.theta())))) / math.cos(math.radians(line.theta()))
+    #print("cx1=%f, cx2=%f" % (cx, cx2))
 
     # "cx_middle" is now the distance from the center of the line. This is our error method to stay
     # on the line. "cx_normal" normalizes the error to something like -1/+1 (it will go over this).
     cx_middle = cx - (img.width() / 2)
     cx_normal = cx_middle / (img.width() / 2)
+    #print("cx=%d, cx_middle=%d, cx_normal=%f" % (cx, cx_middle, cx_normal))
+
+    #exp_cx_normal = (1 - cx_normal) / cx_normal
+    #print("cx_normal=%f, exp_cx_normal=%f" % (cx_normal, exp_cx_normal))
 
     if old_cx_normal != None: old_cx_normal = (cx_normal * MIXING_RATE) + (old_cx_normal * (1.0 - MIXING_RATE))
     else: old_cx_normal = cx_normal
@@ -124,18 +133,19 @@ def figure_out_my_throttle(steering, factor): # steering -> [0:180]
     # sin(180 deg) = 0
 
     # the bigger the factor, the more we slow
-    return max(MIN_THROTTLE, (t_result * THROTTLE_GAIN) + THROTTLE_OFFSET - (factor / 10)) + 15
+    #return max(MIN_THROTTLE, (t_result * THROTTLE_GAIN) + THROTTLE_OFFSET - (factor / 10)) + 15
+    return (t_result * THROTTLE_GAIN) + THROTTLE_OFFSET
 
 # Servo Control Code
 device = pyb.UART(3, 19200, timeout_char = 100)
 
 def read_from_uart():
     read_count = 15
     chars = ""
-    #while device.any() and chars[:-1] != "\n" and read_count > 0:
-        #read_char = str(device.read(1).decode("utf-8"))
-        #chars = chars + read_char
-        #read_count = read_count - 1
+    while device.any() and chars[:-1] != "\n" and read_count > 0:
+        read_char = str(device.read(1).decode("utf-8"))
+        chars = chars + read_char
+        read_count = read_count - 1
     return chars
 
 ## throttle [0:100] (101 values) -> [THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US]
@@ -150,14 +160,14 @@ def set_servos(throttle, steering):
     steering = STEERING_SERVO_MIN_US + ((steering * (STEERING_SERVO_MAX_US - STEERING_SERVO_MIN_US + 1)) / 181)
     device.write("{%05d,%05d}\r\n" % (throttle, steering))
     if DEBUG_PRINT_UART:
-        #if device.any():
-            #chars = read_from_uart()
-
-            #if chars:
-                #print("wrote: {%05d,%05d}, read: %s" % (throttle, steering, chars[:-2] ))
-            #else:
-                #print("wrote: {%05d,%05d}, read: nothing" % (throttle, steering))
-        #else:
+        if device.any():
+            chars = read_from_uart()
+
+            if chars:
+                print("wrote: {%05d,%05d}, read: %s" % (throttle, steering, chars[:-2] ))
+            else:
+                print("wrote: {%05d,%05d}, read: nothing" % (throttle, steering))
+        else:
             print("wrote: {%05d,%05d}, read: nothing" % (throttle, steering))
 
 def invert_steering(steering_in):
@@ -321,12 +331,14 @@ def reset_sensor():
         steering_pid_output = (STEERING_P_GAIN * steering_p_output) + \
                               (STEERING_I_GAIN * steering_i_output) + \
                               (STEERING_D_GAIN * steering_d_output)
+        #print("new_result=%f, delta_result=%f, p_output=%f, i_output=%f, d_output=%f, pid_output + 90=%f" \
+                #% (steering_new_result, steering_delta_result, steering_p_output, steering_i_output, steering_d_output, steering_pid_output+90))
         # STEERING_P_GAIN = -23.0 # Make this smaller as you increase your speed and vice versa.
         # STEERING_I_GAIN = 0.0
         # STEERING_D_GAIN = -9 # Make this larger as you increase your speed and vice versa.
 
         # Steering goes from [-90,90] but we need to output [0,180] for the servos.
-        steering_output = STEERING_OFFSET + max(min(round(steering_pid_output), 180 - STEERING_OFFSET), STEERING_OFFSET - 180)
+        steering_output = STEERING_OFFSET + max(min(round(steering_pid_output * STEERING_FACTOR), 180 - STEERING_OFFSET), STEERING_OFFSET - 180)
 
         # Figure out throttle and do throttle PID
         factor = abs(line.x2() - 90)
@@ -387,7 +399,7 @@ def reset_sensor():
                     steering_output = steering_output - 10 if steering_output < 90 else steering_output + 10
                 steering_output = max(min(steering_output, 180), 0)
         else:
-            throttle_output = throttle_output * .92 if (throttle_output > .1) else 0
+            throttle_output = throttle_output * .95 if (throttle_output > .1) else 0
         print_string = "Line Lost - throttle %d, steering %d" % (throttle_output , steering_output)
 
         pyb.LED(1).on()

led_states.py

@@ -0,0 +1,133 @@
+#led_states.py
+###############################################################
+# Pulses blue LED until April tag is found.
+#
+# Author: Adam A. Koch (aakoch)
+# Date: 2018-04-05
+# Copyright (c) 2018 Adam A. Koch
+# This work is licensed under the MIT license.
+###############################################################
+
+import sensor, image, time, pyb, micropython, math, machine
+from pyb import Timer
+
+sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
+sensor.set_vflip(True)
+sensor.set_hmirror(True)
+sensor.skip_frames(time = 1200)
+sensor.set_auto_gain(False)
+sensor.set_auto_whitebal(False)
+clock = time.clock()
+
+def degrees(radians):
+    return (180 * radians) / math.pi
+
+DEG_120 = micropython.const(round(2 * math.pi / 3)) # 2PI/3 = 120º
+DEG_240 = micropython.const(round(4 * math.pi / 3)) # 4PI/3 = 240º
+RBG_MAX = micropython.const(97)
+PULSE_ALL = micropython.const(0)
+READY_FOR_INPUT = micropython.const(1)
+GOT_INPUT = micropython.const(2)
+
+red_led   = pyb.LED(1)
+green_led = pyb.LED(2)
+blue_led  = pyb.LED(3)
+led_timer = None
+red_channel =  None
+green_channel = None
+blue_channel = None
+
+def leds_on(timer):
+    red_led.on()
+    green_led.on()
+    blue_led.on()
+
+def red_led_off(timer):
+    red_led.off()
+
+def green_led_off(timer):
+    green_led.off()
+
+def blue_led_off(timer):
+    blue_led.off()
+
+def init_led_timers():
+    global led_timer, red_channel, green_channel, blue_channel
+    led_timer = Timer(4, freq=50, callback=leds_on)
+    red_channel =   led_timer.channel(1, Timer.PWM, callback=red_led_off,   pulse_width_percent=0)
+    green_channel = led_timer.channel(2, Timer.PWM, callback=green_led_off, pulse_width_percent=0)
+    blue_channel =  led_timer.channel(3, Timer.PWM, callback=blue_led_off,  pulse_width_percent=0)
+
+def set_pw_colors(i):
+    i = i / 1000
+    if (state == PULSE_ALL):
+        red_on =   max(0, math.sin(i) * RBG_MAX)
+        green_on = max(0, math.sin(i - DEG_120) * RBG_MAX)
+        blue_on =  max(0, math.sin(i - DEG_240) * RBG_MAX)
+
+    elif (state == READY_FOR_INPUT):
+        red_on =   0
+        green_on = 0
+        blue_on = (math.sin(i * 3) + 1.0) / 2.0 * RBG_MAX
+
+    #print("%f %f %f %f" % (i, red_on, blue_on, green_on))
+
+    red_channel.  pulse_width_percent(red_on)
+    green_channel.pulse_width_percent(green_on)
+    blue_channel. pulse_width_percent(blue_on)
+
+def ease(t):
+    sqt = t * t;
+    return sqt / (2.0 * (sqt - t) + 1.0)
+
+
+init_led_timers()
+
+
+def wait_for_april_tag():
+    global state
+    state = READY_FOR_INPUT
+    while (state == READY_FOR_INPUT):
+        set_pw_colors(time.ticks())
+
+        # if not taking snapshots, delay
+        #pyb.udelay(led_timer.period())
+
+        clock.tick() # for fps()
+        img = sensor.snapshot()
+
+        for tag in img.find_apriltags():
+            state = GOT_INPUT
+            sensor.flush()
+        #print(clock.fps())
+    return tag.id();
+
+def acknowledge_input():
+    led_timer.deinit()
+    red_led.off()
+    green_led.on()
+    blue_led.off()
+    pyb.delay(2000)
+    green_led.off()
+
+#def woken_up(rtc):
+    #red_led.toggle()
+    #print("I woke up at %s" % rtc.datetime())
+
+while (True):
+    tagId = wait_for_april_tag()
+    print("Found tag id %d" % tagId)
+
+    acknowledge_input()
+
+    if tagId == 347:
+        pyb.delay(50)
+        machine.reset()
+    elif tagId == 346:
+        #rtc = pyb.RTC()
+        #rtc.wakeup(1000, woken_up)
+        #machine.sleep()
+
+