def dynamic_values_to_csv():
update_cpu_load()
mem = get_mem_usage()
return "%2.1f,%2.1f,%2.1f,%2.1f,%2.1f,%d,%2.1f,%2.1f,%2.1f,%d,%d,%2.2f,%2.2f,%d,%d,%d,%d,%d,%d,%d,%d" % (
cpu_percent_load_array[0][4] * 100, cpu_percent_load_array[1][4] * 100,
cpu_percent_load_array[2][4] * 100, cpu_percent_load_array[3][4] * 100,
cpu_percent_load_array[4][4] * 100, get_arm_clockspeed(),
get_pcb_temp(), get_cpu_temp(), get_gpu_temp(), mem[0], mem[1], mem[2],
get_battery_voltage(), sensors.read_adc(0), sensors.read_adc(1),
sensors.read_adc(2), sensors.read_adc(3), arduino.read_encoder(1),
arduino.read_encoder(3), arduino.read_encoder(2),
arduino.read_encoder(4))
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 right_IR():
distance_right = sensors.get_distance(sensors.read_adc(3),
'2y0a21') / 100.0
return distance_right
def left_IR():
distance_left = sensors.get_distance(sensors.read_adc(2), '2y0a21') / 100.0
#print(distance_top)
return distance_left
def top_IR():
distance_top = sensors.get_distance(sensors.read_adc(0), '2y0a21') / 100.0
#print(distance_top)
return distance_top