# %%
print("Setting arm motors to position 0...")
motor_left_arm.run_to_position(0)
motor_right_arm.run_to_position(0)
print("DONE!")
# %% [markdown]
# # Configure sensor
# %%
print("Configuring sensors...")
color_sensor = ColorSensor('C')
distance_sensor = DistanceSensor('D')
distance_sensor.light_up_all()
print("DONE!")
# %% [markdown]
# # Make Charlie roam around
# %%
print("Roaming...")
# While testing, it can be quite annoying having Charlie moving around.
# If you wish, you can just comment the following line. This way,
# Charlie will stay still while you test your program.
motors_wheels.start()
# %% [markdown]
# # Define distance function
sine_wave_w_params(AMPLITUDE * (1 - DAMPENING * 2), PERIOD,
DELAY_FACTOR * 2),
sine_wave_w_params(AMPLITUDE * (1 - DAMPENING * 3), PERIOD,
DELAY_FACTOR * 3),
]
snake_body = Mechanism(motors, motorfuncs)
while not rcv.is_connected():
print("Waiting for connection...")
sleep_ms(300)
eyes = 100
timer = AMHTimer()
while rcv.is_connected():
speed, turn, delay_setting = rcv.controller_state(R_STICK_VER, L_STICK_HOR,
SETTING2)
if rcv.button_pressed(1):
eyes = 100
if rcv.button_pressed(2):
eyes = 50
if rcv.button_pressed(3):
eyes = 0
ds.light_up_all(eyes)
timer.rate = speed * 20
baseline = turn / 2
snake_body.update_motor_pwms(timer.time,
delay_setting=delay_setting,
baseline=baseline)
raise SystemExit