def program_loop():
global start_time, previous_state, evasion_start_time, evasion_period
current_state = -1
if (previous_state == -1):
if ((time.time() - start_time) > init_period):
current_state = 0
elif ((time.time() - start_time) > loop_period):
start_time = time.time()
min_distance = 100
min_bearing = 0
distances = []
count = 0
for s_def in settings.ROBOT_SENSOR_MODEL:
distance = sensors.get_distance(sensors.read_adc(count), s_def[1])
count += 1
distances.append([s_def[2], distance])
if (distance < min_distance):
min_distance = distance
min_bearing = s_def[2]
#logging.info(distances)
if (min_distance > distance_threshold_high):
current_state = 1
# No obstacle
elif (min_distance < distance_threshold_low):
if (previous_state == 5
and (time.time() - evasion_start_time < evasion_period)):
current_state = 5
else:
current_state = 4
else: #Distance falls between thresholds so decide if it is to left or right
if (min_bearing > 180): current_state = 2
else: current_state = 3
else:
current_state = previous_state #Set current state to previous state when we haven't passed the loop period
if (previous_state != current_state):
logging.info("avoid_obstacles: Current state %d, previous state %d" %
(current_state, previous_state))
if (current_state == 1): #No obstacle; full speed ahead!
motors.forwards(target_speed)
led.set_colour_solid(3) #GREEN
if (current_state == 2): #Obstacle to left, head right
motors.set_motor_speeds(target_speed, target_speed / 2)
led.set_left_colour_pulse(1)
if (current_state == 3):
motors.set_motor_speeds(target_speed / 2, target_speed)
led.set_right_colour_pulse(1)
if (current_state == 4):
evasion_start_time = time.time()
evasion_period = 0.2 + (random.random())
if (random.random() > 0.6):
motors.set_motor_speeds(-target_speed, target_speed)
elif (random.random() > 0.3):
motors.set_motor_speeds(target_speed, -target_speed)
else:
motors.set_motor_speeds(-target_speed / 2, -target_speed / 2)
led.animation(8)
current_state = 5
previous_state = current_state
def robot_speed(speed_l,speed_r):
global moving
motors.set_motor_speeds(speed_l,speed_r)
#arduino.set_motor_speeds(int(speed_l*127),int(speed_r*127))
moving = True
time.sleep(0.1)
def right_button_callback(c, value):
if (c == None): return ('')
motors.set_motor_speeds(value, -value)
arduino.set_motor_speeds(int(value), int(-value))
return ('')
def stop_button_callback(c):
if (c == None): return ('')
motors.set_motor_speeds(0, 0)
arduino.set_motor_speeds(0, 0)
return ('')
def motor_speed_callback(m1, m2):
motors.set_motor_speeds(m1 / 100.0, m2 / 100.0)
return (html.P("Setting motor 1 to %d, motor 2 to %d" % (m1, m2)))