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)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565) # or sensor.GRAYSCALE
sensor.set_framesize(sensor.QVGA) # or sensor.QQVGA (or others)
sensor.skip_frames(time=1500) # Let new settings take affect.
sensor.set_auto_whitebal(False) # Turn off white balance.
sensor.set_auto_gain(False)
clock = time.clock() # Tracks FPS.
COLOR_THRESHOLDS = [(36, 80, -19, 39, -77, -9)
] # for when L is 37, 70, 70, 100
AREA_THRESHOLD = const(20) # Raise to filter out false detections.
PIXELS_THRESHOLD = const(20) # Raise to filter out false detections.
MAG_THRESHOLD = const(3) # Raise to filter out false detections.
device = pyb.UART(3, 19200, timeout_char=100)
throttle = int(ConfigFile().get_property("min_speed")) + 3
def figure_out_my_steering(line, img):
center = round(img.width() / 2)
#angle_1 = remap(line.x1() * 1.1, 0, img.width(), -90, 90)
angle_1 = -math.degrees(
math.atan((center - line.x1()) / (img.height() - BOTTOM_PX_TO_REMOVE)))
#if (line.x1() < img.width()):
#angle_1 = -angle_1
angle_2 = theta(line) / 1.6
#print("angle_1=%.3f, angle_2=%.3f" % (angle_1, angle_2))
steering = angle_1 + angle_2
#steering = remap(line.x1(), 0, img.width(), 180, 0)
# 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):
wdt_reset_str = ConfigFile().get_property("wdt_reset")
error("reset was caused by WDT_RESET")
if (reset_str == None):
num_of_watchdog_resets = 1
else:
num_of_watchdog_resets = int(wdt_reset_str) + 1
error("This has happened", str(num_of_watchdog_resets), "times")
ConfigFile().set_property("wdt_reset", str(num_of_watchdog_resets))
#if (usb_is_connected):
#run_leds(1000)
#else:
#run_leds(10000)
#machine.reset()
#elif not usb_is_connected:
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.B128X128)
#sensor.set_windowing((20, 0, 150, 50))
sensor.set_hmirror(True)
sensor.set_vflip(True)
sensor.skip_frames(time=2000)
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(False)
device = pyb.UART(3, 19200, timeout_char=100)
extra_fb = sensor.alloc_extra_fb(sensor.width(), sensor.height(),
sensor.GRAYSCALE)
#extra_fb = sensor.alloc_extra_fb(150, 50, sensor.GRAYSCALE)
prev_throttle = int(ConfigFile().get_property("min_speed"))
print("Previous min throttle=", str(prev_throttle))
print("Taking pic...")
sensor.skip_frames(time=200)
extra_fb.replace(sensor.snapshot().histeq())
print("Determining noise...")
(x_min, x_max) = (100, -100)
(y_min, y_max) = (100, -100)
start = pyb.millis()
while (pyb.elapsed_millis(start) < 5000):
img = sensor.snapshot().histeq()
displacement = img.find_displacement(extra_fb)
(x_min, x_max) = (min(x_min, displacement.x_translation()),
from pid import SteeringPid
def time_correct():
(year, month, day, weekday, hours, minutes, seconds,
subseconds) = RTC().datetime()
return year >= 2018 and month >= 5 and day >= 5 and hours >= 23 and minutes >= 30
if not time_correct():
print("Time is incorrect.", RTC().datetime())
set_time(2018, 5, 6, 20, 55)
## run the pulse_led.py script if the board was reset because of the watchdog
if (machine.reset_cause() == machine.WDT_RESET):
wdt_reset_str = ConfigFile().get_property("wdt_reset")
error("reset was caused by WDT_RESET")
if (reset_str == None):
num_of_watchdog_resets = 1
else:
num_of_watchdog_resets = int(wdt_reset_str) + 1
error("This has happened", str(num_of_watchdog_resets), "times")
ConfigFile().set_property("wdt_reset", str(num_of_watchdog_resets))
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):
print("min_min=", min_min, ", max_min=", max_min)
print("min_max=", min_max, ", max_max=", max_max)
print("difference", sim)
print("prev min throttle=", prev_throttle)
print("new min throttle=", throttle)
movement_found = True
img.difference(extra_fb)
sensor.flush()
else:
if (pyb.elapsed_millis(start) < 800):
set_servos(throttle)
elif (pyb.elapsed_millis(start) < 1000):
set_servos(prev_throttle - 80)
else:
throttle += 2
start = pyb.millis()
set_servos(prev_throttle - 80)
set_servos(throttle - 80)
ConfigFile().set_property("min_speed", throttle)
img.save("movement_difference" + create_time_based_filename())
for led_count in range(0, 10):
set_servos(throttle - 80)
pyb.LED(2).toggle()
pyb.delay(100)
pyb.LED(2).off()
from file_utils import ConfigFile
i = 00
ConfigFile().set_property("min_speed", i)
print(ConfigFile().get_property("min_speed3"))