def main():
global dist, counter, mode, mode_value, last_mode, current_mode, motor_power
while True:
dist = get_distance()
update_display()
if mode != last_mode:
print('mode', mode, 'Label:', mode_menu[mode])
last_mode = mode
if mode == 0: # standby
stop()
if mode == 1: # collision avoidance
if dist > TOF_MAX_SENSOR_DIST:
# only print if we used to have a valid distance
if valid_distance == 1:
print('no signal')
play_no_signal()
valid_distance = 0
# we have a valid distance
else:
print(dist)
if dist < TURN_DISTANCE:
# back up for a bit
drive_reverse(motor_power)
sleep(REVERSE_TIME)
# turn in a random direction
if random.random() > .5:
print('turning right')
turn_right(motor_power)
else:
print('turning left')
turn_left(motor_power)
sleep(TURN_TIME)
# continue going forward
forward(motor_power)
else:
print('forward')
forward(motor_power)
if mode == 2: # drive square
print('mode 2')
forward(motor_power)
sleep(SQUARE_FWD_TIME)
turn_right(motor_power)
sleep(turn_time)
if mode == 3: # prog power
stop()
motor_power = motor_power_values[mode_value]
print(mode_menu[mode])
if mode == 4: # prog turn dist
print(mode_menu[mode])
if mode == 4: # prog turn dist
print(mode_menu[mode])
sleep(.1)
counter += 1
if mode != current_mode:
print(counter, 'mode:', mode, 'mode val:', mode_value)
current_mode = mode
def main():
global dist, counter, mode, last_mode, current_mode, motor_power, turn_distance, reverse_time, turn_time
while True:
dist = get_tof_distance_cm()
update_display()
if mode == 0: # standby
stop()
elif mode == 1: # collision avoidance
if dist > TOF_MAX_SENSOR_DIST:
# only print if we used to have a valid distance
if valid_distance == 1:
print('no signal')
play_no_signal()
valid_distance = 0
# we have a valid distance
else:
print(dist)
if dist < turn_distance:
# back up for a bit
drive_reverse(motor_power)
sleep(reverse_time)
# turn in a random direction
if random.random() > .5:
print('turning right')
turn_right(motor_power)
else:
print('turning left')
turn_left(motor_power)
sleep(turn_time)
# continue going forward
forward(motor_power)
else:
print('forward')
forward(motor_power)
elif mode == 2: # drive square
print('mode 2')
forward(motor_power)
sleep(SQUARE_FWD_TIME)
turn_right(motor_power)
sleep(turn_time)
elif mode == 3: # prog power
stop()
sleep(.1)
# counter += 1
if mode != current_mode:
print('mode:', mode)
current_mode = mode
sleep(.1)
counter += 1
if mode != current_mode:
print(counter, 'mode:', mode, 'mode val:', mode_value)
current_mode = mode
mode_value = 0
# if (counter % 50) == 0:
# print(counter, 'mode:', mode, 'mode val:', mode_value)
# startup
tof = VL53L0X.VL53L0X(i2c)
tof.start()
# play_startup()
# This allows us to stop the sound by doing a Stop or Control-C which is a keyboard intrrupt
print('Running Collision Avoidence with Time-of-Flight Sensor Version 3.1')
try:
main()
except KeyboardInterrupt:
print('Got ctrl-c')
finally:
# Optional cleanup code
print('turning off sound')
sound_off()
print('turning off motors')
stop()
tof.stop()