add theta_rho black and white

read_analog_pin6.py

@@ -8,11 +8,12 @@
 # Copyright (c) 2018 Adam A. Koch
 # This work is licensed under the MIT license.
 ###############################################################
+import pyb
 from pyb import ADC
 
 adc = ADC("P6") # Must always be "P6".
 
 while(True):
     # The ADC has 12-bits of resolution for 4096 values.
-    print("ADC = %d" % round(adc.read() / 200))
-    time.sleep(100)
+    print("ADC = %d" % adc.read())
+    pyb.delay(100)

read_json_qrcode.py

@@ -0,0 +1,21 @@
+# QRCode Example
+#
+# Reads a QRCode and then uses ujson to convert JSON into object
+
+import sensor, image, ujson
+
+sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QVGA)
+sensor.set_vflip(True)
+sensor.set_hmirror(True)
+sensor.skip_frames(time = 2000)
+sensor.set_auto_gain(False) # must turn this off to prevent image washout...
+
+while(True):
+    img = sensor.snapshot()
+    img.lens_corr(1.8) # strength of 1.8 is good for the 2.8mm lens.
+    for code in img.find_qrcodes():
+        img.draw_rectangle(code.rect(), color = (255, 0, 0))
+        obj = ujson.loads(str(code.payload()))
+        print(obj);

theta_rho_bw.py

@@ -0,0 +1,49 @@
+#######################################################################
+# Draws a line matching the line found using get_regression, then draws
+# a circle with the same diameter as rho. The values for rho and theta
+# are drawn too.
+#
+# Author: Adam A. Koch (aakoch)
+# Date: 2018-04-08
+# Copyright (c) 2018 Adam A. Koch
+# This work is licensed under the MIT license.
+#
+# See
+# https://github.com/openmv/openmv/blob/master/usr/examples/09-Feature-Detection/linear_regression_robust.py
+#######################################################################
+
+import sensor, image, time, pyb
+
+THRESHOLD = [(88, 147)] # works for my masking tape on my chalkboard wall
+
+sensor.reset()
+# if you have your camera mounted on a Donkey car, flip the image
+#sensor.set_vflip(True)
+#sensor.set_hmirror(True)
+sensor.set_pixformat(sensor.GRAYSCALE)
+sensor.set_framesize(sensor.QQQVGA)
+sensor.skip_frames(time = 2000)
+clock = time.clock()
+
+while(True):
+    clock.tick()
+    img = sensor.snapshot()
+
+    line = img.get_regression(THRESHOLD, robust = True)
+
+    if (line):
+        img.draw_line(line.line(), 255)
+        rho = line.rho()
+        theta = line.theta()
+
+        # we convert any negative rho to a positive rho and add 180 degrees so we can pass a
+        # positive value as the radius when drawing our circle later
+        if rho < 0:
+            rho = abs(rho)
+            theta += 180
+
+        print("theta=%f, rho=%f, x1=%f, x2=%f, y1=%f, y2=%f" %  \
+            (theta, rho, line.x1(), line.x2(), line.y1(), line.y2()))
+
+        img.draw_circle(0, 0, rho, 240)
+        img.draw_string(img.width() - 80, 0, "theta=%f\n  rho=%f" % (theta, rho), 240)

throttle_backward_test.py

@@ -0,0 +1,106 @@
+# This file is part of the OpenMV project.
+# 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
+from pyb import Pin, Timer
+from pyb import ADC
+from file_utils import ConfigFile
+#from util_functions import *
+
+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
+###########
+
+
+# Tweak these values for your robocar.
+THROTTLE_SERVO_MIN_US = 1268 # from output_throttle Arduino sketch
+THROTTLE_SERVO_MAX_US = 1692
+
+# Tweak these values for your robocar.
+STEERING_SERVO_MIN_US = 1276
+STEERING_SERVO_MAX_US = 1884
+
+# 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.
+
+
+# Servo Control Code
+device = pyb.UART(3, 19200, timeout_char = 100)
+
+## 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):
+    throttle = remap(throttle, -100, 100, THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
+    #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)
+
+    steering = STEERING_SERVO_MIN_US + ((steering * (STEERING_SERVO_MAX_US - STEERING_SERVO_MIN_US + 1)) / 181)
+    device.write("{%05d,%05d}\r\n" % (throttle, steering))
+    print("wrote: {%05d,%05d}" % (throttle, steering))
+
+def invert_steering(steering_in):
+    return ((steering_in - 90) * -1) + 90
+
+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()
+
+start_time = pyb.millis()
+
+adc = ADC("P6") # Must always be "P6".
+i = -50
+throttle_output = i
+
+
+read_value = ConfigFile().get_property("min_speed")
+print("min_speed = " + read_value if read_value else "unknown")
+
+pyb.delay(500)
+pulse_sensor_prev = adc.read()
+sensed_movement = False
+while not sensed_movement:
+
+    print("throttle %d, steering %d" % (throttle_output , 100))
+    set_servos(throttle_output, 100)
+
+    pulse_sensor = adc.read()
+    sensed_movement =  abs(pulse_sensor_prev - pulse_sensor) > 100
+    print("pulse_sensor_prev = %d, pulse_sensor = %d, sensed_movement = %s" % (pulse_sensor_prev, pulse_sensor, sensed_movement))
+    pulse_sensor_prev = round((pulse_sensor_prev + pulse_sensor) / 2.0)
+
+    if i < -65:
+        print("hit failsafe")
+        sensed_movement = True
+    else:
+        if (sensed_movement):
+            print("moved at %d" % (i + 1))
+        else:
+            throttle_output = i
+            i -= 1
+            pyb.delay(200)
+
+for j in range(5):
+    set_servos(0, 100)
+    pyb.delay(200)
+
+ConfigFile().set_property("min_speed", i)

throttle_test.py

@@ -0,0 +1,109 @@
+#throttle_test.py
+###############################################################
+# Find the lowest throttle value the car will move.
+# This only works on specialized code on the Arduino. ***
+# The steering output of the Arduino interferes with the
+# pulse sensor readings. You can re-program the Arduino to
+# not output then you have to re-program it for regular
+# runing of the car.
+#
+# Author: Adam A. Koch (aakoch)
+# Date: 2018-03-22
+# Copyright (c) 2018 Adam A. Koch
+# This work is licensed under the MIT license.
+###############################################################
+
+import sensor, image, time, math, pyb, uio
+from pyb import Pin, Timer
+from pyb import ADC
+from file_utils import ConfigFile
+#from util_functions import *
+
+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
+###########
+
+# Tweak these values for your robocar.
+THROTTLE_SERVO_MIN_US = 1268 # from output_throttle Arduino sketch
+THROTTLE_SERVO_MAX_US = 1692
+
+# Tweak these values for your robocar.
+STEERING_SERVO_MIN_US = 1276
+STEERING_SERVO_MAX_US = 1884
+
+# 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.
+
+
+# Servo Control Code
+device = pyb.UART(3, 19200, timeout_char = 100)
+
+## 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):
+    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)
+
+    device.write("{%05d,%05d}\r\n" % (throttle, steering))
+    print("wrote: {%05d,%05d}" % (throttle, steering))
+
+def invert_steering(steering_in):
+    return ((steering_in - 90) * -1) + 90
+
+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()
+
+start_time = pyb.millis()
+
+adc = ADC("P6") # Must always be "P6".
+i = 0
+throttle_output = i
+
+print("min_speed = " + ConfigFile().get_property("min_speed"))
+
+pyb.delay(500)
+pulse_sensor_prev = adc.read()
+sensed_movement = False
+while not sensed_movement:
+
+    print("throttle %d, steering %d" % (throttle_output , 100))
+    set_servos(throttle_output, 100)
+
+    pulse_sensor = adc.read()
+    sensed_movement =  abs(pulse_sensor_prev - pulse_sensor) > 50
+    print("pulse_sensor_prev = %d, pulse_sensor = %d, sensed_movement = %s" % (pulse_sensor_prev, pulse_sensor, sensed_movement))
+    pulse_sensor_prev = round((pulse_sensor_prev + pulse_sensor) / 2.0)
+
+    if i > 45:
+        print("hit failsafe")
+        sensed_movement = True
+    else:
+        if (sensed_movement):
+            print("moved at %d" % (i - 1))
+        else:
+            throttle_output = max(min(round(i), 100), 0)
+            i += 1
+            pyb.delay(500)
+
+for j in range(5):
+    set_servos(0, 100)
+    pyb.delay(200)
+
+ConfigFile().set_property("min_speed", i)