def _write_config_file(run_as_user):
caps = list_caps()
if 'Credentials' not in caps:
log.warning('Cannot obtain Jupyter password; will bail.')
sys.exit(1)
creds = acquire('Credentials')
jupyter_password = None
while True:
jupyter_password = creds.get_jupyter_password()
if jupyter_password is not None:
break
log.info('Waiting for Jupyter password to be set...')
time.sleep(1)
release(creds)
log.info('Got Jupyter password.')
user_home_dir_path = pwd.getpwnam(run_as_user).pw_dir
with open(JUPYTER_CONFIG_TEMPLATE, 'r') as f_template:
template = f_template.read()
with open(JUPYTER_CONFIG_OUTPUT, 'w') as f_out:
final_config = template
final_config = final_config.replace(r'<JUPYTER_PASSWORD>',
repr(jupyter_password))
final_config = final_config.replace(r'<JUPYTER_START_DIR>',
repr(user_home_dir_path))
f_out.write(final_config)
def _button_numeric_input(prompt,
initial=0,
adj_delta=1,
change_callback=None):
TIMER_RESET_VAL = 15
adj_factor = [1, 10, 100] # increase increment/decrement amount by factor
adj = adj_rate = extra_delay = val_idx = 0
vals = [initial] if not isinstance(initial, list) else initial
val = vals[val_idx]
still_editing = True
held = False
c.print(prompt)
buttons = acquire('PushButtons')
while still_editing:
events = buttons.get_events()
if events:
for e in events:
if _is_released(_BUTTONS['SUBMIT'], e):
if val_idx + 1 == len(vals):
still_editing = False
else:
val_idx += 1
val = vals[val_idx]
if _is_released(_BUTTONS['LEFT/DOWN'], e) or _is_released(
_BUTTONS['RIGHT/UP'], e):
held = False
adj = 0
adj_rate = 0
extra_delay = 0
if _is_pressed(_BUTTONS['LEFT/DOWN'], e):
held = True
timer = TIMER_RESET_VAL
adj = -adj_delta
if _is_pressed(_BUTTONS['RIGHT/UP'], e):
held = True
timer = TIMER_RESET_VAL
adj = adj_delta
val += adj * adj_factor[adj_rate]
else:
val += adj * (adj_factor[adj_rate])
if isinstance(adj_delta, float):
val = round(val, len(str(adj_delta)) - 2)
if held:
if timer != 0:
timer -= 1
else:
if val % (10 * adj_delta) == 0:
adj_rate = min([adj_rate + 1, len(adj_factor)])
extra_delay = 0.50 # give more time for humans to react
timer = TIMER_RESET_VAL
vals[val_idx] = val
output = f"[ {' / '.join([f'({v})' if i == val_idx else f' {v} ' for i, v in enumerate(vals)])} ]"
if change_callback is not None:
change_callback(vals)
c.print(output, end='\r')
time.sleep(0.05 + extra_delay)
c.print('Finished choosing', vals)
release(buttons)
return vals[0] if len(vals) == 1 else ' '.join(str(v) for v in vals)
def _physics_honk(count):
global _PHYSICS
try:
_PHYSICS
except NameError:
_PHYSICS = acquire('PhysicsClient')
_PHYSICS.control({
'type': 'honk',
'count': count,
})
def _get_pid_steering():
global _PID_STEERING
try:
_PID_STEERING
except NameError:
caps = list_caps()
if 'PID_steering' not in caps:
raise AttributeError('This device has no PID steering loop.')
_PID_STEERING = acquire('PID_steering')
return _PID_STEERING
def _get_gyro_accum():
global _GYRO_ACCUM
try:
_GYRO_ACCUM
except NameError:
caps = list_caps()
if 'Gyroscope_accum' not in caps:
raise AttributeError('This device has no gyroscope.')
_GYRO_ACCUM = acquire('Gyroscope_accum')
return _GYRO_ACCUM
def _get_motors():
global _MOTORS
try:
_MOTORS
except NameError:
caps = list_caps()
if 'CarMotors' not in caps:
raise AttributeError('This device is not a car.')
_MOTORS = acquire('CarMotors')
_MOTORS.on()
return _MOTORS
def _get_encoder():
global _ENCODER
try:
_ENCODER
except NameError:
caps = list_caps()
if 'Encoders' not in caps:
raise AttributeError('This device has no encoder.')
_ENCODER = acquire('Encoders')
_ENCODER.enable(ENCODER_INDEX)
return _ENCODER
def _get_one_button_press():
buttons = acquire('PushButtons')
try:
while True:
events = buttons.get_events()
presses = [e['button'] for e in events if e['action'] == 'pressed']
if presses:
return presses[0]
time.sleep(0.1)
finally:
release(buttons)
def _find_servo_range(steering_direction, io_device):
params = _query_motor_params()
steering_mid = params['steering_mid']
gyro = acquire('Gyroscope')
def detect_max_steering(f_throttle, r_throttle, db_field, steering_val, start_offset, stride, sign, sign2):
def compute_avg_gz(test_val):
STORE.put(db_field, test_val)
_setup_motors()
m.MOTORS.set_steering(steering_val)
m.MOTORS.set_throttle(f_throttle)
time.sleep(0.2)
avg_gz = 0.0
i = 0
t_start = time.time()
while time.time() - t_start < 0.4:
m.MOTORS.set_steering(steering_val)
m.MOTORS.set_throttle(f_throttle)
_, _, z = gyro.read()
avg_gz += z
i += 1
m.MOTORS.set_throttle(0.0)
return avg_gz / i
prev_gz = None
for offset in itertools.count(start_offset, stride):
avg_gz = compute_avg_gz(int(round(steering_mid + offset * sign)))
time.sleep(0.5)
m.MOTORS.set_steering(steering_val)
m.MOTORS.set_throttle(r_throttle)
time.sleep(0.6)
m.MOTORS.set_throttle(0.0)
time.sleep(0.5)
if prev_gz is not None:
pct_change = (avg_gz - prev_gz) / prev_gz
if pct_change * sign2 < 0.05:
answer = int(round(steering_mid + (offset - stride) * sign)) # The previous value.
STORE.put(db_field, answer)
_setup_motors(save=True)
break
prev_gz = avg_gz
return answer
f_throttle = m.CAR_THROTTLE_FORWARD_SAFE_SPEED
r_throttle = m.CAR_THROTTLE_REVERSE_SAFE_SPEED
steering_left = detect_max_steering(f_throttle, r_throttle, 'CAR_MOTOR_STEERING_LEFT', 45.0, 300, 100, steering_direction, 1.0)
steering_right = detect_max_steering(f_throttle, r_throttle, 'CAR_MOTOR_STEERING_RIGHT', -45.0, 300, 100, -steering_direction, 1.0)
release(gyro)
def buzz(notes):
"""
Play the given `notes` on the device's buzzer.
"""
global _BUZZER
try:
_BUZZER
except NameError:
caps = list_caps()
if 'Buzzer' not in caps:
raise AttributeError("This device does not have a buzzer.")
_BUZZER = acquire('Buzzer')
_BUZZER.play(notes)
_BUZZER.wait()
def _calibrate_microcontroller(io_device):
time.sleep(2) # Allow user a few seconds to put the device down.
calibrator = acquire('Calibrator')
print_func = {
'computer': print,
'car' : c.print,
}[io_device]
pinwheel = _spinning_pinwheel()
for status in calibrator.calibrate():
if status == '.':
print_func(f'Calibrating [{next(pinwheel)}]', end='\r')
else:
print_func(status)
if status == 'Finished microcontroller calibration!':
break
time.sleep(1)
release(calibrator)
buzzer.honk() # Let the user know the calibration finished!
def global_camera(verbose=False):
"""
Creates (for the first call) or retrieves (for later calls) the
global camera object. This is a convenience function to facilitate
quickly and easily creating and retrieving a camera singleton.
"""
global GLOBAL_CAMERA
try:
GLOBAL_CAMERA
except NameError:
caps = list_caps()
if 'Camera' not in caps:
raise AttributeError("This device does not have a Camera.")
camera = acquire('Camera')
GLOBAL_CAMERA = _CameraRGB(camera)
if verbose:
auto._ctx_print_all("Instantiated a global camera object!")
return GLOBAL_CAMERA
def get_servo(servo_index, frequency=50, min_duty=0.025, max_duty=0.125):
"""
Acquire the interface to the servo at index `servo_index`.
The first servo is at index 0.
"""
global _PWMs
try:
_PWMs
except NameError:
caps = list_caps()
if 'PWMs' not in caps:
raise AttributeError("This device does not have a PWM controller.")
_PWMs = acquire('PWMs')
n_pins = _PWMs.num_pins()
if servo_index < 0 or servo_index >= n_pins:
raise Exception(
"Invalid servo index given: {}. This device has servos 0 through {}"
.format(servo_index, n_pins - 1))
return _ServoTemplate(_PWMs, servo_index, frequency, min_duty, max_duty)
def _button_choice_input(prompt, choices):
c.print(prompt)
choice_i = 0
decided = False
buttons = acquire('PushButtons')
while not decided:
events = buttons.get_events()
for e in events:
if _is_released(_BUTTONS['RIGHT/UP'], e):
choice_i += 1
elif _is_released(_BUTTONS['LEFT/DOWN'], e):
choice_i -= 1
elif _is_released(_BUTTONS['SUBMIT'], e):
decided = True
break
if abs(choice_i) == len(choices):
choice_i = 0
output = f"[ {' / '.join([(f'({c})' if i == abs(choice_i) else f' {c} ') for i, c in enumerate(choices)])} ]"
c.print(output, end='\r' if not decided else '\n')
time.sleep(0.05)
release(buttons)
return choices[choice_i]
def _find_okay_speed_and_mid_steering(io_device):
accelerometer = acquire('Accelerometer')
gyro_accum = acquire('Gyroscope_accum')
gyro = acquire('Gyroscope')
f_min_throttle_start, f_max_throttle_start = 1, 35
r_min_throttle_start, r_max_throttle_start = -1, -35
speed_target = 1.3 # m/s
gz_drift_proportional = 10.0
def test_once(min_throttle, max_throttle):
m.MOTORS.set_steering(0.0)
m.MOTORS.set_throttle(0.0)
_, _, gz_start = gyro_accum.read()
throttle = (max_throttle + min_throttle) / 2
speed_est = 0.0
t_start = time.time()
t_curr = t_start
while True:
m.MOTORS.set_throttle(throttle)
_, y, _ = accelerometer.read()
t_prev, t_curr = t_curr, time.time()
speed_est += (t_curr - t_prev) * y * 9.81
if abs(speed_est) > speed_target * 1.05:
break
if t_curr - t_start >= 1.0:
break
_, _, gz_end = gyro_accum.read()
gz_drift = (gz_end - gz_start) / (t_curr - t_start)
m.MOTORS.set_throttle(0.0)
if abs(speed_est) > speed_target:
max_throttle = throttle
else:
min_throttle = throttle
done = (abs(max_throttle - min_throttle) < 2.0)
time.sleep(1) # Give time for the car to stop...
return throttle, min_throttle, max_throttle, speed_est, done, gz_drift
def detect_steering_direction(f_throttle, r_throttle):
def compute_avg_gz(steering_mid):
STORE.put('CAR_MOTOR_STEERING_MID', steering_mid)
_setup_motors()
m.MOTORS.set_steering(0.0)
m.MOTORS.set_throttle(f_throttle)
time.sleep(0.05)
avg_gz = 0.0
i = 0
t_start = time.time()
while time.time() - t_start < 0.3:
m.MOTORS.set_steering(0.0)
m.MOTORS.set_throttle(f_throttle)
_, _, z = gyro.read()
avg_gz += z
i += 1
m.MOTORS.set_throttle(0.0)
return avg_gz / i
params = _query_motor_params()
steering_mid = params['steering_mid']
results = []
for offset in [-500, 500, 0]:
avg_gz = compute_avg_gz(steering_mid + offset)
time.sleep(0.5)
m.MOTORS.set_steering(0.0)
m.MOTORS.set_throttle(r_throttle)
time.sleep(0.35)
m.MOTORS.set_throttle(0.0)
time.sleep(0.5)
results.append(avg_gz)
if results[1] > results[0]:
return 1.0
else:
return -1.0
f_min_throttle, f_max_throttle, f_done = \
f_min_throttle_start, f_max_throttle_start, False
r_min_throttle, r_max_throttle, r_done = \
r_min_throttle_start, r_max_throttle_start, False
steering_direction = None
while not f_done or not r_done:
f_throttle, f_min_throttle, f_max_throttle, f_speed_est, f_done, f_gz_drift = \
test_once(f_min_throttle, f_max_throttle)
# print(f_min_throttle, f_max_throttle, f_speed_est, f_done, f_gz_drift)
r_throttle, r_min_throttle, r_max_throttle, r_speed_est, r_done, r_gz_drift = \
test_once(r_min_throttle, r_max_throttle)
# print(r_min_throttle, r_max_throttle, r_speed_est, r_done, r_gz_drift)
if f_speed_est > 0.1 and r_speed_est < -0.1:
# This is backwards. Negative y points forward on our cars.
# So, swap the underlying values.
# Also, start the binary search over, since the data to now isn't good.
params = _query_motor_params()
STORE.put('CAR_MOTOR_THROTTLE_FORWARD', params['throttle_reverse'])
STORE.put('CAR_MOTOR_THROTTLE_REVERSE', params['throttle_forward'])
_setup_motors(save=True)
f_min_throttle, f_max_throttle, f_done = \
f_min_throttle_start, f_max_throttle_start, False
r_min_throttle, r_max_throttle, r_done = \
r_min_throttle_start, r_max_throttle_start, False
elif f_speed_est < -0.1 and r_speed_est > 0.1:
# We have some speed, so we can think about steering now.
if abs(f_gz_drift) > 25.0 or abs(r_gz_drift) > 25.0:
# Our steering is too far off. Start over the search.
f_min_throttle, f_max_throttle, f_done = \
f_min_throttle_start, f_max_throttle_start, False
r_min_throttle, r_max_throttle, r_done = \
r_min_throttle_start, r_max_throttle_start, False
if abs(f_gz_drift) > 5.0 or abs(r_gz_drift) > 5.0:
f_done, r_done = False, False
if steering_direction is None:
steering_direction = detect_steering_direction(f_throttle, r_throttle)
params = _query_motor_params()
steering_mid = params['steering_mid']
# print(steering_mid, f_gz_drift, r_gz_drift)
if abs(f_gz_drift) > abs(r_gz_drift):
offset = steering_direction * f_gz_drift * gz_drift_proportional
else:
offset = steering_direction * -r_gz_drift * gz_drift_proportional
STORE.put('CAR_MOTOR_STEERING_MID', int(round(steering_mid - offset)))
_setup_motors()
f_throttle = (f_min_throttle + f_max_throttle) / 2
r_throttle = (r_min_throttle + r_max_throttle) / 2
# print(f_throttle, r_throttle)
m.CAR_THROTTLE_FORWARD_SAFE_SPEED = f_throttle
m.CAR_THROTTLE_REVERSE_SAFE_SPEED = r_throttle
STORE.put('CAR_THROTTLE_FORWARD_SAFE_SPEED', f_throttle)
STORE.put('CAR_THROTTLE_REVERSE_SAFE_SPEED', r_throttle)
_setup_motors(save=True)
release(gyro)
release(gyro_accum)
release(accelerometer)
return steering_direction
from auto.capabilities import list_caps, acquire
from itertools import count
import time
b = acquire('Power')
start_time = time.time()
with open('batlog.csv', 'wt') as f:
f.write('index,time,millivolts\n')
for i in count():
v = b.millivolts()
line = '{},{},{}'.format(i, time.time() - start_time, v)
print(line)
f.write(line + '\n')
f.flush()
wait_time = start_time + i + 1 - time.time()
time.sleep(wait_time)
###############################################################################
import time
from collections import deque
from auto.capabilities import list_caps, acquire, release
from auto import logger
log = logger.init('battery_monitor', terminal=True)
log.info("Starting menu driver process...")
buttons = acquire("PushButtons")
button_stream = deque()
combo = [1, 2, 3, 3, 2, 1]
def _run_menu():
# TODO: This should, someday, be a nice menu of options shown on the LCD screen.
# For now, however, we'll just run the calibration routine.
# In the future, the calibration routine will be _one_ of the menu options.
from car.calibration import calibrate
calibrate('car') # <-- blocks until calibration is finished
while True:
def _hone_speed(io_device):
f_throttle = m.CAR_THROTTLE_FORWARD_SAFE_SPEED
r_throttle = m.CAR_THROTTLE_REVERSE_SAFE_SPEED
gyro_accum = acquire('Gyroscope_accum')
def compute_avg_gz(throttle, steering, delay):
m.MOTORS.set_steering(steering)
m.MOTORS.set_throttle(throttle)
time.sleep(0.2) # allow time to change to happen (speed up, slow down, ...)
_, _, gy_start = gyro_accum.read()
time.sleep(delay)
_, _, gy_end = gyro_accum.read()
return (gy_end - gy_start) / delay
def find_perfect_throttle(throttle, steering, target_gz):
m.MOTORS.set_steering(steering)
m.MOTORS.set_throttle(throttle)
time.sleep(0.5) # allow time for car to speed up
while True:
gz = compute_avg_gz(throttle, steering, 0.4)
if throttle < 0:
gz = -gz
if abs((gz - target_gz) / target_gz) < 0.05:
return throttle
if target_gz > 0 and gz < target_gz or target_gz < 0 and gz > target_gz:
throttle *= 1.05
else:
throttle /= 1.05
while True:
if _choice_input(prompt="Will go forward and left. Ready?",
choices=['n', 'y'],
io_device=io_device) == 'y':
forward_left_throttle = find_perfect_throttle(f_throttle, 45.0, 120)
if _choice_input(prompt="Did the car move freely?",
choices=['n', 'y'],
io_device=io_device) == 'y':
break
while True:
if _choice_input(prompt="Will go forward and right. Ready?",
choices=['n', 'y'],
io_device=io_device) == 'y':
forward_right_throttle = find_perfect_throttle(f_throttle, -45.0, -120)
if _choice_input(prompt="Did the car move freely?",
choices=['n', 'y'],
io_device=io_device) == 'y':
break
while True:
if _choice_input(prompt="Will go reverse and left. Ready?",
choices=['n', 'y'],
io_device=io_device) == 'y':
reverse_left_throttle = find_perfect_throttle(r_throttle, 45.0, 100)
if _choice_input(prompt="Did the car move freely?",
choices=['n', 'y'],
io_device=io_device) == 'y':
break
while True:
if _choice_input(prompt="Will go reverse and right. Ready?",
choices=['n', 'y'],
io_device=io_device) == 'y':
reverse_right_throttle = find_perfect_throttle(r_throttle, -45.0, -100)
if _choice_input(prompt="Did the car move freely?",
choices=['n', 'y'],
io_device=io_device) == 'y':
break
f_throttle = (forward_left_throttle + forward_right_throttle) / 2
r_throttle = (reverse_left_throttle + reverse_right_throttle) / 2
m.CAR_THROTTLE_FORWARD_SAFE_SPEED = f_throttle
m.CAR_THROTTLE_REVERSE_SAFE_SPEED = r_throttle
STORE.put('CAR_THROTTLE_FORWARD_SAFE_SPEED', f_throttle)
STORE.put('CAR_THROTTLE_REVERSE_SAFE_SPEED', r_throttle)
release(gyro_accum)