saving good state

boot.py

@@ -0,0 +1,50 @@
+#boot.py
+###############################################################
+# First thing ran by robot
+#
+# Author: Adam A. Koch (aakoch)
+# Date: 2018-04-05
+# Copyright (c) 2018 Adam A. Koch
+# This work is licensed under the MIT license.
+###############################################################
+
+import machine, pyb
+from machine import WDT
+from constants import *
+from reset_functions import *
+from class_camera import *
+from settings import Settings
+
+# Steps when starting
+# 1) Boot
+# 2) Settings
+# 3) Init
+# 4) Run
+
+# this will check, and then if the reset was due to the watchdog, it will run the pulse LED file
+check_reset_cause()
+
+#def setup_settings():
+    #settings = Settings()
+    #return settings
+
+#settings = setup_settings()
+
+camera = Camera()
+
+#if (settings.is_watchdog_enabled):
+    #watchdog = WDT(timeout=5000)
+    #camera.set_watchdog(watchdog)
+
+#camera.set_camera_pixel_format_to_rgb()
+#camera.set_camera_thresholds(Threshold.BLUE)
+
+for n in range(50):
+    line = camera.find_line()
+
+    if line and (line.magnitude() >= MAG_THRESHOLD):
+        print(line.magnitude())
+
+# ...
+
+# run

donkey.py

@@ -2,10 +2,50 @@
 # Copyright (c) 2013-2017 Ibrahim Abdelkader <[email protected]> & Kwabena W. Agyeman <[email protected]>
 # This work is licensed under the MIT license, see the file LICENSE for details.
 
-import sensor, image, time, math, pyb, uio
+import sensor, image, time, math, pyb, uio, machine, sys, gc
 from pyb import Pin, Timer
+from pyb import ADC
+from file_utils import ConfigFile
+from machine import WDT
+from pulse_led import run_leds
+from micropython import const
 
 
+usb = pyb.USB_VCP() # This is a serial port object that allows you to
+# communciate with your computer. While it is not open the code below runs.
+usb_is_connected = usb.isconnected()
+usb = None
+
+#if (machine.reset_cause() == machine.PWRON_RESET):
+    #print("reset was caused by PWRON_RESET")
+#elif (machine.reset_cause() == machine.HARD_RESET):
+    #print("reset was caused by HARD_RESET")
+#elif (machine.reset_cause() == machine.DEEPSLEEP_RESET):
+    #print("reset was caused by DEEPSLEEP_RESET")
+#elif (machine.reset_cause() == machine.SOFT_RESET):
+    #print("reset was caused by SOFT_RESET")
+
+
+# run the pulse_led.py script if the board was reset because of the watchdog
+if (machine.reset_cause() == machine.WDT_RESET):
+    print("reset was caused by WDT_RESET")
+    if (usb_is_connected):
+        run_leds(1000)
+    else:
+        run_leds(10000)
+        machine.reset()
+
+elif not usb_is_connected:
+    wdt = WDT(timeout=10000)  # enable it with a timeout of 10s
+
+# after running LEDs?
+gc.collect()
+
+def remap(x, in_min, in_max, out_min, out_max):
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
+def constrain(val, min_val, max_val):
+    return min(max_val, max(min_val, val))
+
 ###########
 # Settings
 ###########
@@ -18,7 +58,9 @@
 DEBUG_LINE_STATUS = True
 #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
+#COLOR_THRESHOLDS = [(1, 80, -25, 28, -58, -10)] # Blue tape line
+#COLOR_THRESHOLDS = [(1, 80, -25, 37, -76, -47)] # Blue tape line 2 - upstairs - sunny
+COLOR_THRESHOLDS = [(1, 80, -25, 37, -76, -9)] # Blue tape line 2 - upstairs - clowdy
 #kitchen table:
 #COLOR_THRESHOLDS = [(30, 78, -18, 28, -70, -19)]
 GRAYSCALE_THRESHOLDS = [(240, 255)] # White Line.
@@ -27,43 +69,46 @@
 FRAME_SIZE = sensor.QQVGA # Frame size.
 FRAME_REGION = 0.8 # Percentage of the image from the bottom (0 - 1.0).
 FRAME_WIDE = 1.0 # Percentage of the frame width.
-BOTTOM_PX_TO_REMOVE = 4 # maybe I screwed something up with my camera, but the last few rows are just noise
+BOTTOM_PX_TO_REMOVE = const(4) # maybe I screwed something up with my camera, but the last few rows are just noise
 
-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.
+AREA_THRESHOLD = const(20) # Raise to filter out false detections.
+PIXELS_THRESHOLD = const(20) # Raise to filter out false detections.
+MAG_THRESHOLD = const(6) # Raise to filter out false detections.
 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 = 30 # e.g. default speed (0 to 100)
+THROTTLE_CUT_OFF_ANGLE = 2.5 # Maximum angular distance from 90 before we cut speed [0.0-90.0).
+THROTTLE_CUT_OFF_RATE = .6 # How much to cut our speed boost (below) once the above is passed (0.0-1.0].
+THROTTLE_GAIN = 11.0 # e.g. how much to speed up on a straight away
+
+#read_value = ConfigFile().get_property("min_speed")
+#if read_value:
+    #THROTTLE_OFFSET = int(read_value)
+#else:
+THROTTLE_OFFSET = 52 # e.g. default speed (0 to 100)
 THROTTLE_P_GAIN = 1.0
 THROTTLE_I_GAIN = 0.0
 THROTTLE_I_MIN = -0.0
 THROTTLE_I_MAX = 0.0
 THROTTLE_D_GAIN = 0.0
-MIN_THROTTLE = 10
+#MIN_THROTTLE = 51
 
 # Tweak these values for your robocar.
-STEERING_FACTOR = 1.2 # 1-
-STEERING_OFFSET = 100 # Change this if you need to fix an imbalance in your car (0 to 180).
+STEERING_FACTOR = 1 # 1-
+STEERING_OFFSET = 90 # 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_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 = 2100
-THROTTLE_SERVO_MIN_US = 1500
-THROTTLE_SERVO_MAX_US = 2000
+THROTTLE_SERVO_MIN_US = const(1268) # from output_throttle Arduino sketch
+THROTTLE_SERVO_MAX_US = const(1692)
 
 # Tweak these values for your robocar.
-STEERING_SERVO_MIN_US = 1276
-STEERING_SERVO_MAX_US = 1884
+STEERING_SERVO_MIN_US = const(1276)
+STEERING_SERVO_MAX_US = const(1884)
 
 FRAME_REGION = max(min(FRAME_REGION, 1.0), 0.0)
 FRAME_WIDE = max(min(FRAME_WIDE, 1.0), 0.0)
@@ -72,19 +117,6 @@
 THROTTLE_CUT_OFF_ANGLE = max(min(THROTTLE_CUT_OFF_ANGLE, 89.99), 0)
 THROTTLE_CUT_OFF_RATE = max(min(THROTTLE_CUT_OFF_RATE, 1.0), 0.01)
 
-THROTTLE_OFFSET = max(min(THROTTLE_OFFSET, 100), 0)
-STEERING_OFFSET = max(min(STEERING_OFFSET, 180), 0)
-
-# Handle if these were reversed...
-tmp = max(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
-THROTTLE_SERVO_MIN_US = min(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
-THROTTLE_SERVO_MAX_US = tmp
-
-# Handle if these were reversed...
-tmp = max(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US)
-STEERING_SERVO_MIN_US = min(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US)
-STEERING_SERVO_MAX_US = tmp
-
 # This function maps the output of the linear regression function to a driving vector for steering
 # the robocar. See https://openmv.io/blogs/news/linear-regression-line-following for more info.
 
@@ -93,6 +125,12 @@
 def figure_out_my_steering(line, img):
     global old_cx_normal
 
+    #return line.theta() / 180
+
+    #print(line.theta())
+    #if (line.x1() < 80 and line.x2() > 80) or (line.x1() > 80 and line.x2() < 80):
+        #return line.theta()
+
     # Rho is computed using the inverse of this code below in the actual OpenMV Cam code.
     # This formula comes from the Hough line detection formula (see the wikipedia page for more).
     # Anyway, the output of this calculations below are a point centered vertically in the middle
@@ -142,22 +180,18 @@ def figure_out_my_throttle(steering, factor): # steering -> [0:180]
 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
+    if not usb_is_connected:
+        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]
-# throttle [-100:100] (201 values) -> [THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US]
-# steering [0:180] (181 values) -> [STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US]
+
 def set_servos(throttle, steering):
-    if throttle < 0:
-        throttle = 1000 + ((throttle * (THROTTLE_SERVO_MIN_US - 1000 + 1)) / 101)
-    else:
-        throttle = THROTTLE_SERVO_MIN_US + ((throttle * (THROTTLE_SERVO_MAX_US - THROTTLE_SERVO_MIN_US + 1)) / 101)
+    throttle = remap(throttle, -100, 100, THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
+    steering = remap(steering, 0, 180, STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US)
 
-    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():
@@ -191,7 +225,6 @@ def turn_green_led_off(timer):
 green_led_timer.callback(turn_green_led_on)          # set the callback to our tick function
 green_led_timer_channel = green_led_timer.channel(1, Timer.PWM, callback=turn_green_led_off, pulse_width_percent=50)
 
-
 #green_led_on = False
 #magnitude = 0
 
@@ -256,9 +289,6 @@ def reset_sensor():
 else:
     f = None
 
-usb = pyb.USB_VCP() # This is a serial port object that allows you to
-# communciate with your computer. While it is not open the code below runs.
-usb_is_connected = usb.isconnected()
 
 line_lost_count = 0
 delta_time = 0
@@ -267,9 +297,15 @@ def reset_sensor():
 jump_start_counter = 10
 use_hist = True
 
+adc = ADC("P6") # Must always be "P6".
+
 while True:
     clock.tick()
 
+    if not usb_is_connected:
+        wdt.feed()
+    #print("ADC = %d" % adc.read())
+
     if line_lost_count > 5 and use_hist:
         use_hist = False
 
@@ -278,7 +314,6 @@ def reset_sensor():
         line_lost_count = 0
         use_hist = True
 
-
     if use_hist:
         img = sensor.snapshot().histeq()
     #elif use_binary:
@@ -310,10 +345,10 @@ def reset_sensor():
         delta_time = new_time - old_time
         old_time = new_time
 
-        if delta_time > 110:
-            pyb.LED(3).on()
-        else:
-            pyb.LED(3).off()
+        #if delta_time > 110:
+            #pyb.LED(3).on()
+        #else:
+            #pyb.LED(3).off()
 
         # Figure out steering and do steering PID
         steering_new_result = figure_out_my_steering(line, img)
@@ -338,7 +373,8 @@ def reset_sensor():
         # 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 * STEERING_FACTOR), 180 - STEERING_OFFSET), STEERING_OFFSET - 180)
+        steering_output = STEERING_OFFSET + max(min(round(steering_pid_output), 180 - STEERING_OFFSET), STEERING_OFFSET - 180)
+        #steering_output_mapped = remap(steering_pid_output, -90, 90, STEERING_OFFSET - 90, STEERING_OFFSET + 90)
 
         # Figure out throttle and do throttle PID
         factor = abs(line.x2() - 90)
@@ -366,6 +402,8 @@ def reset_sensor():
         if jump_start_counter > 0:
             throttle_output = throttle_output + JUMP_START_THROTTLE_INCREASE
 
+        # y1 is ALWAYS 0
+        # y2 is ALWAYS 91
         print_string = "Line Ok - throttle %d, steering %d - line t: %d°, r: %d, x1: %d, y1: %d, x2: %d, y2: %d, 1/2: %d, mag: %d" % \
             (throttle_output , steering_output, line.theta(), line.rho(), line.x1(), line.y1(), line.x2(), line.y2(), sensor.width() / 2, line.magnitude())
         #tup = (min(line.x1(), line.x2()), line.y1(), abs(line.x1() - line.x2()), abs(line.y1() - line.y2()))

menu.py

@@ -0,0 +1,65 @@
+#menu.py
+###############################################################
+# Writing down thoughts.
+#
+# Author: Adam A. Koch (aakoch)
+# Date: 2018-04-06
+# Copyright (c) 2018 Adam A. Koch
+# This work is licensed under the MIT license.
+###############################################################
+
+import sensor, image, time, sys, constants
+from class_camera import *
+from class_threshold import *
+from constants import *
+from machine import WDT
+
+#sensor.reset()
+#sensor.set_pixformat(sensor.RGB565)
+#sensor.set_framesize(sensor.QVGA)
+#sensor.skip_frames(time = 2000)
+
+#clock = time.clock()
+
+#while(True):
+    #clock.tick()
+    #img = sensor.snapshot()
+    #print(clock.fps())
+
+
+# Settings:
+# 1) B/W
+# 2) Color
+# 3) Yellow thresholds
+# 4) Blue thresholds
+# 5) Enable watchdog
+# 6) Disable watchdog
+# 7) Enable recording
+# 8) Disable recording
+# 9) Calibrate throttle
+# 10) Calibrate steering
+
+
+camera = Camera()
+
+watchdog = WDT(timeout=5000)
+camera.set_watchdog(watchdog)
+
+#camera.set_camera_pixel_format_to_rgb()
+#camera.set_camera_thresholds(Threshold.BLUE)
+
+for n in range(50):
+    line = camera.find_line()
+
+    if line and (line.magnitude() >= MAG_THRESHOLD):
+        print(line.magnitude())
+
+# Steps when starting
+# 1) Boot
+# 2) Settings
+# 3) Init
+# 4) Run
+
+while(True):
+    watchdog.feed()
+    pyb.delay(4000)