def mutlithreading():
"""
In the first example below a thread called twenty_tones is started - it plays twenty tones
while the main script waits for the touch sensor button to be 'bumped' (pressed and released)
5 times. The idea is that bumping five times will cause the tone-playing to be interrupted.
But if you try running the script you will observe that in this case the playing of the tones
is NOT interrupted if you bump the sensor 5 times. However, if the tones stop before the five
bumps have been finished then program execution stops after the bumps (and the beep) have been
completed, as you would expect.
"""
ts = TouchSensor()
sound = Sound()
def twenty_tones():
for j in range(0, 20): # Do twenty times.
sound.play_tone(1000, 0.2) # 1000Hz for 0.2s
sleep(0.5)
t = Thread(target=twenty_tones)
t.start()
for i in range(0, 5): # Do five times, with i = 0, 1, 2, 3, 4.
ts.wait_for_bump()
sound.beep()
class Ev3rstorm(IRBeaconRemoteControlledTank):
def __init__(self,
left_foot_motor_port: str = OUTPUT_B,
right_foot_motor_port: str = OUTPUT_C,
bazooka_blast_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
super().__init__(left_motor_port=left_foot_motor_port,
right_motor_port=right_foot_motor_port,
motor_class=LargeMotor,
ir_sensor_port=ir_sensor_port,
ir_beacon_channel=ir_beacon_channel)
self.bazooka_blast_motor = MediumMotor(
address=bazooka_blast_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.screen = Display()
self.speaker = Sound()
def blast_bazooka_whenever_touched(self):
self.screen.image_filename(filename='/home/robot/image/Target.bmp',
clear_screen=True)
self.screen.update()
while True:
self.touch_sensor.wait_for_bump()
if self.color_sensor.ambient_light_intensity < 5: # 15 not dark enough
self.speaker.play_file(wav_file='/home/robot/sound/Up.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
self.bazooka_blast_motor.on_for_rotations(speed=100,
rotations=-3,
brake=True,
block=True)
else:
self.speaker.play_file(wav_file='/home/robot/sound/Down.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
self.bazooka_blast_motor.on_for_rotations(speed=100,
rotations=3,
brake=True,
block=True)
def main(self, driving_speed: float = 100):
Process(target=self.blast_bazooka_whenever_touched,
daemon=True).start()
self.keep_driving_by_ir_beacon(speed=driving_speed)
sound.speak("O, A, O, A")
print("OKOKOKOKOKOKOKOKOKOKOK")
'''
while True:
if ts.is_pressed:
mt.off()
break
if btn.check_buttons(buttons=['up']):
mt.on(-50, -50)
elif btn.check_buttons(buttons=['up']) == False:
mt.off()
'''
ts.wait_for_bump()
sound.play_tone(2500, 0.1)
os.system("echo 0 > RESTART_TRAFFIC_LIGHT")
print("RESTART_TRAFFIC_LIGHT")
'''
sleep(0.1)
os.system("echo 0 > RESTART_TRAFFIC_LIGHT")
#!/usr/bin/env python3
from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C, MoveSteering
from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor
from time import sleep
from ev3dev2.sound import Sound
from threading import Thread
us = UltrasonicSensor()
us.mode = 'US-DIST-CM'
ts = TouchSensor()
sound = Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
speed = 0
while True:
if ts.wait_for_bump() == True:
speed += 10
steer_pair.on(steering=0, speed=speed)
else:
sleep(0.01)
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Robot state
self.patrol_mode = False
self.follow_mode = False
# Internal Variables
self.light_intensity = 0
self.batt_voltage = 0
# Connect two large motors on output ports B and C
#self.drive = MoveTank(OUTPUT_D, OUTPUT_C)
self.steerdrive = MoveSteering(OUTPUT_C, OUTPUT_D)
self.leds = Leds()
self.ir = InfraredSensor()
self.ir.mode = 'IR-SEEK'
self.touch = TouchSensor()
self.light = ColorSensor(address='ev3-ports:in4')
self.sound = Sound()
# Start threads
threading.Thread(target=self._patrol_thread, daemon=True).start()
threading.Thread(target=self._follow_thread, daemon=True).start()
threading.Thread(target=self._pat_thread, daemon=True).start()
threading.Thread(target=self._power_thread, daemon=True).start()
threading.Thread(target=self._light_sensor_thread, daemon=True).start()
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]), int(payload["speed"]))
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
if control_type == "follow":
self.follow_mode = True
if control_type == "stopfollow":
self.follow_mode = False
except KeyError:
print("Missing expected parameters: {}".format(directive), file=sys.stderr)
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking), file=sys.stderr)
if direction in Direction.FORWARD.value:
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking)
if direction in Direction.BACKWARD.value:
self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(-speed), duration, block=is_blocking)
if direction in (Direction.RIGHT.value + Direction.LEFT.value):
self._turn(direction, speed)
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking)
if direction in Direction.STOP.value:
self.drive.off()
self.patrol_mode = False
def _activate(self, command, speed=50):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed), file=sys.stderr)
if command in Command.MOVE_CIRCLE.value:
self.drive.on_for_seconds(SpeedPercent(int(speed)), SpeedPercent(5), 12)
if command in Command.MOVE_SQUARE.value:
for i in range(4):
self._move("right", 2, speed, is_blocking=True)
if command in Command.PATROL.value:
# Set patrol mode to resume patrol thread processing
self.patrol_mode = True
if command in Command.SENTRY.value:
self.sentry_mode = True
self._send_event(EventName.SPEECH, {'speechOut': "Sentry mode activated"})
# Perform Shuffle posture
self.drive.on_for_seconds(SpeedPercent(80), SpeedPercent(-80), 0.2)
time.sleep(0.3)
self.drive.on_for_seconds(SpeedPercent(-40), SpeedPercent(40), 0.2)
self.leds.set_color("LEFT", "YELLOW", 1)
self.leds.set_color("RIGHT", "YELLOW", 1)
def _turn(self, direction, speed):
"""
Turns based on the specified direction and speed.
Calibrated for hard smooth surface.
:param direction: the turn direction
:param speed: the turn speed
"""
if direction in Direction.LEFT.value:
self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2)
if direction in Direction.RIGHT.value:
self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2)
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _patrol_thread(self):
"""
Performs random movement when patrol mode is activated.
"""
while True:
while self.patrol_mode:
print("Patrol mode activated randomly picks a path", file=sys.stderr)
direction = random.choice(list(Direction))
duration = random.randint(1, 5)
speed = random.randint(1, 4) * 25
while direction == Direction.STOP:
direction = random.choice(list(Direction))
# direction: all except stop, duration: 1-5s, speed: 25, 50, 75, 100
self._move(direction.value[0], duration, speed)
time.sleep(duration)
time.sleep(1)
def _pat_thread(self):
"""
Detects when the touch sensor is pressed.
"""
while True:
self.touch.wait_for_bump()
sound = "Ahh, I like that."
self._send_event(EventName.SPEECH, {'speechOut': sound})
def _light_sensor_thread(self):
"""
"""
while True:
self.light.mode='COL-AMBIENT'
time.sleep(0.5)
self.light_intensity = self.light.ambient_light_intensity
if self.batt_voltage < 3.6:
# Set the LED to be red.
self.light.mode='REF-RAW'
else:
self.light.mode='COL-COLOR'
# Set the LED to be white.
print("Light Intensity: ", self.light_intensity)
time.sleep(5)
def _follow_thread(self):
"""
The thread to manage following the lease.
"""
while True:
if self.follow_mode:
# Get heading to beacon
heading = self.ir.heading()
print("IR Heading: ", heading)
# Can't see the beacon
if heading == 0:
time.sleep(1)
continue
drive_dir = -heading
# Drive
self.steerdrive.on_for_rotations(drive_dir, SpeedPercent(30), 2, block=True)
time.sleep(1)
def _power_thread(self):
"""
Sends power output to Alexa skill.
"""
charge_current_pid = 'FIXME'
load_current_pid = 'FIXME'
batt_voltage_pid = 'FIXME'
time.sleep(2)
while True:
try:
# list properties_v2
api_response = api_instance.properties_v2_show(id, batt_voltage_pid)
print('Battery Voltage: ', round(api_response.last_value, 3))
voltage = round(api_response.last_value, 3)
voltage = 3.54
self.batt_voltage = voltage
except ApiException as e:
print("Exception when calling PropertiesV2Api->propertiesV2List: %s\n" % e)
try:
api_response = api_instance.properties_v2_show(id, load_current_pid)
print('Load Current: ', round(api_response.last_value, 2))
load_current = round(api_response.last_value, 1)
except ApiException as e:
print("Exception when calling PropertiesV2Api->propertiesV2List: %s\n" % e)
try:
api_response = api_instance.properties_v2_show(id, charge_current_pid)
print('Charge Current: ', round(api_response.last_value, 2))
charge_current = round(api_response.last_value, 1)
except ApiException as e:
print("Exception when calling PropertiesV2Api->propertiesV2List: %s\n" % e)
time.sleep(15)
self._send_event(EventName.POWER, {'voltage': voltage, 'load_current': load_current, 'charge_current': charge_current, 'light':self.light_intensity })
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import TouchSensor
from random import randint
from time import sleep
ts = TouchSensor(INPUT_1)
m1 = LargeMotor(OUTPUT_A)
m2 = LargeMotor(OUTPUT_B)
m3 = LargeMotor(OUTPUT_C)
print("ready!")
ts.wait_for_bump() # wait for user to calibrate motors
m1.reset()
m2.reset()
m3.reset()
while True:
ts.wait_for_bump()
m1.on(speed=randint(-100, 100))
m2.on(speed=randint(-100, 100))
m3.on(speed=randint(-100, 100))
ts.wait_for_bump()
m1.off(brake=False)
ts.wait_for_bump()
m2.off(brake=False)
ts.wait_for_bump()
class Robot:
def __init__(self):
self.sound = Sound()
self.direction_motor = MediumMotor(OUTPUT_D)
self.swing_motorL = LargeMotor(OUTPUT_A)
self.swing_motorC = LargeMotor(OUTPUT_B)
self.swing_motorR = LargeMotor(OUTPUT_C)
self.swing_motors = [
self.swing_motorL, self.swing_motorC, self.swing_motorR
]
self.touch_sensor = TouchSensor(INPUT_1)
self.console = Console(DEFAULT_FONT)
self.buttons = Button()
self.beeps_enabled = True
def beep(self, frequency=700, wait_for_comeplete=False):
if not self.beeps_enabled:
return
play_type = Sound.PLAY_WAIT_FOR_COMPLETE if wait_for_comeplete else Sound.PLAY_NO_WAIT_FOR_COMPLETE
self.sound.beep("-f %i" % frequency, play_type=play_type)
def calibrate_dir(self):
''' Calibrate direction motor '''
# Run motor with 25% power, and wait until it stops running
self.direction_motor.on(SpeedPercent(-10), block=False)
# while (not self.direction_motor.STATE_OVERLOADED):
# print(self.direction_motor.duty_cycle)
self.direction_motor.wait_until(self.direction_motor.STATE_OVERLOADED)
self.direction_motor.stop_action = Motor.STOP_ACTION_COAST
self.direction_motor.stop()
time.sleep(.5)
# Reset to straight
# self.direction_motor.on_for_seconds(SpeedPercent(10), .835, brake=True, block=True)
self.direction_motor.on_for_degrees(SpeedPercent(10),
127,
brake=True,
block=True)
self.direction_motor.reset()
print("Motor reset, position: " + str(self.direction_motor.position))
time.sleep(.5)
def calibrate_swing(self):
for m in self.swing_motors:
m.stop_action = m.STOP_ACTION_HOLD
m.on(SpeedPercent(6))
self.swing_motorC.wait_until(self.swing_motorC.STATE_OVERLOADED, 2000)
for m in self.swing_motors:
m.stop_action = m.STOP_ACTION_HOLD
m.on_for_degrees(SpeedPercent(5), -15, brake=True, block=False)
self.swing_motorC.wait_while('running')
for m in self.swing_motors:
m.reset()
m.stop_action = m.STOP_ACTION_HOLD
m.stop()
print("Ready Angle: %i" % self.swing_motorC.position)
def ready_swing(self, angle: int):
right_angle = -(angle / 3)
# adjust motors to target angle
for m in self.swing_motors:
m.stop_action = Motor.STOP_ACTION_HOLD
m.on_for_degrees(SpeedPercent(2),
right_angle,
brake=True,
block=False)
self.swing_motorC.wait_while('running')
for m in self.swing_motors:
m.stop_action = Motor.STOP_ACTION_HOLD
m.stop()
print("Swing Angle: %i" % self.swing_motorC.position)
def set_direction(self, direction):
print("Setting direction to: " + str(direction))
#direction = self.__aim_correction(direction)
self.direction_motor.on_for_degrees(SpeedPercent(10),
round(direction * 3))
print("Direction set to: " + str(self.direction_motor.position))
#
# def __aim_correction(self, direction):
# x = direction
# y = -0.00000000429085685725*x**6 + 0.00000004144345630728*x**5 + 0.00001219331494759860*x**4 + 0.00020702946527870400*x**3 + 0.00716486965517779000*x**2 + 1.29675836037884000000*x + 0.27064829453014400000
#
# return y
def shoot(self, power):
print("SHOOT, power: %i" % power)
for m in self.swing_motors:
m.duty_cycle_sp = -power
for m in self.swing_motors:
m.run_direct()
time.sleep(.5)
self.swing_motorC.wait_until_not_moving()
for m in self.swing_motors:
m.reset()
def wait_for_button(self):
self.touch_sensor.wait_for_bump()
def __set_display(self, str):
self.console.set_font(font=LARGER_FONT, reset_console=True)
self.console.text_at(str, column=1, row=1, reset_console=True)
def wait_for_power_select(self, power=0, angle=0, steps=1):
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
def left():
power -= steps
if power < 0:
power = 0
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['left'], timeout_ms=150)
def right():
power += steps
if power > 100:
power = 100
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['right'], timeout_ms=150)
def up():
angle += steps
if angle > 110:
angle = 110
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['up'], timeout_ms=150)
def down():
angle -= steps
if angle < 0:
angle = 0
self.__set_display("Pow: %i\nAngle: %i" % (power, angle))
self.buttons.wait_for_released(buttons=['down'], timeout_ms=150)
while not self.touch_sensor.is_pressed:
if self.buttons.left:
left()
elif self.buttons.right:
right()
elif self.buttons.up:
up()
elif self.buttons.down:
down()
self.console.set_font(font=DEFAULT_FONT, reset_console=True)
return (power, angle)
def select_connection_mode(self):
self.__set_display("Enable Connection\nLeft: True - Right: False")
enabled = True
while not self.touch_sensor.is_pressed:
if self.buttons.left:
enabled = True
self.buttons.wait_for_released(buttons=['left'])
break
elif self.buttons.right:
enabled = False
self.buttons.wait_for_released(buttons=['right'])
break
self.console.set_font(font=DEFAULT_FONT, reset_console=True)
return enabled
def print(self, string):
print(string)
print('Up button pressed' if state else 'Up button released')
def down(state):
print('Down button pressed' if state else 'Down button released')
def enter(state):
print(color_list.append('Yellow') if state else sleep(0.01))
btn.on_left = left
btn.on_right = right
btn.on_up = up
btn.on_down = down
btn.on_enter = enter
color_list = [] # create empty list
sound.beep()
while ts.wait_for_bump() == False:
btn.process()
sleep(0.01)
#for i in range(4): # i=0 then 1 then 2 then 3
sound.beep()
ts.wait_for_bump()
#sound.play_file('/home/robot/sounds/Horn 2.wav')
for col in color_list:
if col=='Blue':
sound.beep() # blue: turn the robot 90 degrees left
steer_pair.on(steering=0,speed=10)
sleep(1)
elif col=='Green':
sound.beep() # green: go straight forward for one wheel rotation
steer_pair.on(steering=0,speed=30)
sleep(1)
class BalancerLearner(object):
"""
Base class for a robot that stands on two wheels and uses a gyro sensor.
Robot will keep its balance using reinforcement learning
"""
def __init__(self,
gain_gyro_angle=1700,
gain_gyro_rate=120,
gain_motor_angle=7,
gain_motor_angular_speed=9,
gain_motor_angle_error_accumulated=3,
power_voltage_nominal=8.0,
pwr_friction_offset_nom=3,
timing_loop_msec=30,
motor_angle_history_length=5,
gyro_drift_compensation_factor=0.05,
left_motor_port=OUTPUT_D,
right_motor_port=OUTPUT_A,
debug=False):
"""Create GyroBalancer."""
# Gain parameters
self.gain_gyro_angle = gain_gyro_angle
self.gain_gyro_rate = gain_gyro_rate
self.gain_motor_angle = gain_motor_angle
self.gain_motor_angular_speed = gain_motor_angular_speed
self.gain_motor_angle_error_accumulated =\
gain_motor_angle_error_accumulated
# Power parameters
self.power_voltage_nominal = power_voltage_nominal
self.pwr_friction_offset_nom = pwr_friction_offset_nom
# Timing parameters
self.timing_loop_msec = timing_loop_msec
self.motor_angle_history_length = motor_angle_history_length
self.gyro_drift_compensation_factor = gyro_drift_compensation_factor
# Power supply setup
self.power_supply = PowerSupply()
# Gyro Sensor setup
self.gyro = GyroSensor()
#self.gyro.mode = self.gyro.MODE_GYRO_RATE
self.gyro.mode = self.gyro.MODE_GYRO_G_A
# Touch Sensor setup
self.touch = TouchSensor()
# IR Buttons setup
# self.remote = InfraredSensor()
# self.remote.mode = self.remote.MODE_IR_REMOTE
# Configure the motors
self.motor_left = LargeMotor(left_motor_port)
self.motor_right = LargeMotor(right_motor_port)
# Sound setup
self.sound = Sound()
# Open sensor and motor files
self.gyro_angle_file = open(self.gyro._path + "/value0", "rb")
self.gyro_rate_file = open(self.gyro._path + "/value1", "rb")
self.touch_file = open(self.touch._path + "/value0", "rb")
self.encoder_left_file = open(self.motor_left._path + "/position",
"rb")
self.encoder_right_file = open(self.motor_right._path + "/position",
"rb")
self.dc_left_file = open(self.motor_left._path + "/duty_cycle_sp", "w")
self.dc_right_file = open(self.motor_right._path + "/duty_cycle_sp",
"w")
# Drive queue
self.drive_queue = queue.Queue()
# Stop event for balance thread
self.stop_balance = threading.Event()
# Debugging
self.debug = debug
# Handlers for SIGINT and SIGTERM
signal.signal(signal.SIGINT, self.signal_int_handler)
signal.signal(signal.SIGTERM, self.signal_term_handler)
def shutdown(self):
"""Close all file handles and stop all motors."""
self.stop_balance.set() # Stop balance thread
self.motor_left.stop()
self.motor_right.stop()
self.gyro_angle_file.close()
self.gyro_rate_file.close()
self.touch_file.close()
self.encoder_left_file.close()
self.encoder_right_file.close()
self.dc_left_file.close()
self.dc_right_file.close()
def _fast_read(self, infile):
"""Function for fast reading from sensor files."""
infile.seek(0)
return(int(infile.read().decode().strip()))
def _fast_write(self, outfile, value):
"""Function for fast writing to motor files."""
outfile.truncate(0)
outfile.write(str(int(value)))
outfile.flush()
def _set_duty(self, motor_duty_file, duty, friction_offset,
voltage_comp):
"""Function to set the duty cycle of the motors."""
# Compensate for nominal voltage and round the input
duty_int = int(round(duty*voltage_comp))
# Add or subtract offset and clamp the value between -100 and 100
if duty_int > 0:
duty_int = min(100, duty_int + friction_offset)
elif duty_int < 0:
duty_int = max(-100, duty_int - friction_offset)
# Apply the signal to the motor
self._fast_write(motor_duty_file, duty_int)
def signal_int_handler(self, signum, frame):
"""Signal handler for SIGINT."""
log.info('"Caught SIGINT')
self.shutdown()
raise GracefulShutdown()
def signal_term_handler(self, signum, frame):
"""Signal handler for SIGTERM."""
log.info('"Caught SIGTERM')
self.shutdown()
raise GracefulShutdown()
def balance(self):
"""Run the _balance method as a thread."""
balance_thread = threading.Thread(target=self._balance)
balance_thread.start()
def _balance(self):
"""Make the robot balance."""
while True and not self.stop_balance.is_set():
# Reset the motors
self.motor_left.reset() # Reset the encoder
self.motor_right.reset()
self.motor_left.run_direct() # Set to run direct mode
self.motor_right.run_direct()
# Initialize variables representing physical signals
# (more info on these in the docs)
# The angle of "the motor", measured in raw units,
# degrees for the EV3).
# We will take the average of both motor positions as
# "the motor" angle, which is essentially how far the middle
# of the robot has travelled.
motor_angle_raw = 0
# The angle of the motor, converted to RAD (2*pi RAD
# equals 360 degrees).
motor_angle = 0
# The reference angle of the motor. The robot will attempt to
# drive forward or backward, such that its measured position
motor_angle_ref = 0
# equals this reference (or close enough).
# The error: the deviation of the measured motor angle from the
# reference. The robot attempts to make this zero, by driving
# toward the reference.
motor_angle_error = 0
# We add up all of the motor angle error in time. If this value
# gets out of hand, we can use it to drive the robot back to
# the reference position a bit quicker.
motor_angle_error_acc = 0
# The motor speed, estimated by how far the motor has turned in
# a given amount of time.
motor_angular_speed = 0
# The reference speed during manouvers: how fast we would like
# to drive, measured in RAD per second.
motor_angular_speed_ref = 0
# The error: the deviation of the motor speed from the
# reference speed.
motor_angular_speed_error = 0
# The 'voltage' signal we send to the motor.
# We calculate a new value each time, just right to keep the
# robot upright.
motor_duty_cycle = 0
# The raw value from the gyro sensor in rate mode.
gyro_rate_raw = 0
# The angular rate of the robot (how fast it is falling forward
# or backward), measured in RAD per second.
gyro_rate = 0
# The gyro doesn't measure the angle of the robot, but we can
# estimate this angle by keeping track of the gyro_rate value
# in time.
gyro_est_angle = 0
# Over time, the gyro rate value can drift. This causes the
# sensor to think it is moving even when it is perfectly still.
# We keep track of this offset.
gyro_offset = 0
# Start
log.info("Hold robot upright. Press touch sensor to start.")
self.sound.speak("Press touch sensor to start balancing")
self.touch.wait_for_bump()
# Read battery voltage
voltage_idle = self.power_supply.measured_volts
voltage_comp = self.power_voltage_nominal / voltage_idle
# Offset to limit friction deadlock
friction_offset = int(round(self.pwr_friction_offset_nom *
voltage_comp))
# Timing settings for the program
# Time of each loop, measured in seconds.
loop_time_target = self.timing_loop_msec / 1000
loop_count = 0 # Loop counter, starting at 0
# A deque (a fifo array) which we'll use to keep track of
# previous motor positions, which we can use to calculate the
# rate of change (speed)
motor_angle_hist =\
deque([0], self.motor_angle_history_length)
# The rate at which we'll update the gyro offset (precise
# definition given in docs)
gyro_drift_comp_rate =\
self.gyro_drift_compensation_factor *\
loop_time_target * RAD_PER_SEC_PER_RAW_GYRO_UNIT
# Calibrate Gyro
log.info("-----------------------------------")
log.info("Calibrating...")
# As you hold the robot still, determine the average sensor
# value of 100 samples
gyro_calibrate_count = 100
for i in range(gyro_calibrate_count):
gyro_data = self._fast_read(self.gyro_rate_file)
#log.info(gyro_data)
#gyro_other_data = self._fast_read(self.gyro_other)
#log.info(gyro_other_data)
gyro_offset = gyro_offset + gyro_data
#self.gyro_data_file.write(str(gyro_data), "\n")
#self.gyro_data_file.flush()
time.sleep(0.01)
gyro_offset = gyro_offset / gyro_calibrate_count
#gyro_offset = 0
# Print the result
log.info("gyro_offset: " + str(gyro_offset))
log.info("-----------------------------------")
log.info("GO!")
log.info("-----------------------------------")
log.info("Press Touch Sensor to re-start.")
log.info("-----------------------------------")
self.sound.beep()
# Remember start time
prog_start_time = time.time()
if self.debug:
# Data logging
data = OrderedDict()
loop_times = OrderedDict()
data['loop_times'] = loop_times
gyro_angles = OrderedDict()
data['gyro_angles'] = gyro_angles
gyro_rates_raw = OrderedDict()
data['gyro_rates_raw'] = gyro_rates_raw
gyro_est_angles = OrderedDict()
data['gyro_est_angles'] = gyro_est_angles
gyro_rates = OrderedDict()
data['gyro_rates'] = gyro_rates
motor_angle_errors = OrderedDict()
data['motor_angle_errors'] = motor_angle_errors
motor_angular_speed_errors = OrderedDict()
data['motor_angular_speed_errors'] = motor_angular_speed_errors
motor_angle_errors_acc = OrderedDict()
data['motor_angle_errors_acc'] = motor_angle_errors_acc
motor_duty_cycles = OrderedDict()
data['motor_duty_cycles'] = motor_duty_cycles
# Initial fast read touch sensor value
touch_pressed = False
# Driving and Steering
speed, steering = (0, 0)
# Record start time of loop
loop_start_time = time.time()
# Balancing Loop
while not touch_pressed and not self.stop_balance.is_set():
loop_count += 1
# Check for drive instructions and set speed / steering
try:
speed, steering = self.drive_queue.get_nowait()
self.drive_queue.task_done()
except queue.Empty:
pass
# Read the touch sensor (the kill switch)
touch_pressed = self._fast_read(self.touch_file)
# Read the Motor Position
motor_angle_raw = ((self._fast_read(self.encoder_left_file) +
self._fast_read(self.encoder_right_file)) /
2.0)
motor_angle = motor_angle_raw * RAD_PER_RAW_MOTOR_UNIT
# Read the Gyro
gyro_rate_raw = self._fast_read(self.gyro_rate_file)
#log.info(gyro_rate_raw)
# Busy wait for the loop to reach target time length
loop_time = 0
while(loop_time < loop_time_target):
loop_time = time.time() - loop_start_time
time.sleep(0.001)
# Calculate most recent loop time
loop_time = time.time() - loop_start_time
# Set start time of next loop
loop_start_time = time.time()
#if self.debug:
# Log gyro data and loop time
#time_of_sample = time.time() - prog_start_time
#gyro_angles[time_of_sample] = self._fast_read(self.gyro_angle_file)
#gyro_rates[time_of_sample] = gyro_rate_raw
#loop_times[time_of_sample] = loop_time * 1000.0
# Calculate gyro rate
gyro_rate = (gyro_rate_raw - gyro_offset) *\
RAD_PER_SEC_PER_RAW_GYRO_UNIT
# Calculate Motor Parameters
motor_angular_speed_ref =\
speed * RAD_PER_SEC_PER_PERCENT_SPEED
motor_angle_ref = motor_angle_ref +\
motor_angular_speed_ref * loop_time_target
motor_angle_error = motor_angle - motor_angle_ref
# Compute Motor Speed
motor_angular_speed =\
((motor_angle - motor_angle_hist[0]) /
(self.motor_angle_history_length * loop_time_target))
motor_angular_speed_error = motor_angular_speed
motor_angle_hist.append(motor_angle)
# Compute the motor duty cycle value
motor_duty_cycle =\
(self.gain_gyro_angle * gyro_est_angle +
self.gain_gyro_rate * gyro_rate +
self.gain_motor_angle * motor_angle_error +
self.gain_motor_angular_speed *
motor_angular_speed_error +
self.gain_motor_angle_error_accumulated *
motor_angle_error_acc)
# Apply the signal to the motor, and add steering
self._set_duty(self.dc_right_file, motor_duty_cycle + steering,
friction_offset, voltage_comp)
self._set_duty(self.dc_left_file, motor_duty_cycle - steering,
friction_offset, voltage_comp)
# Update angle estimate and gyro offset estimate
gyro_est_angle = gyro_est_angle + gyro_rate *\
loop_time_target
gyro_offset = (1 - gyro_drift_comp_rate) *\
gyro_offset + gyro_drift_comp_rate * gyro_rate_raw
# Update Accumulated Motor Error
motor_angle_error_acc = motor_angle_error_acc +\
motor_angle_error * loop_time_target
if self.debug:
# Log gyro data and loop time
time_of_sample = time.time() - prog_start_time
gyro_angles[time_of_sample] = self._fast_read(self.gyro_angle_file)
gyro_rates_raw[time_of_sample] = gyro_rate_raw
loop_times[time_of_sample] = loop_time * 1000.0
motor_duty_cycles[time_of_sample] = motor_duty_cycle
gyro_est_angles[time_of_sample] = gyro_est_angle
gyro_rates[time_of_sample] = gyro_rate
motor_angle_errors[time_of_sample] = motor_angle_error
motor_angular_speed_errors[time_of_sample] = motor_angular_speed_error
motor_angle_errors_acc[time_of_sample] = motor_angle_error_acc
# Closing down & Cleaning up
# Loop end time, for stats
prog_end_time = time.time()
# Turn off the motors
self._fast_write(self.dc_left_file, 0)
self._fast_write(self.dc_right_file, 0)
# Wait for the Touch Sensor to be released
while self.touch.is_pressed:
time.sleep(0.01)
# Calculate loop time
avg_loop_time = (prog_end_time - prog_start_time) / loop_count
log.info("Loop time:" + str(avg_loop_time * 1000) + "ms")
# Print a stop message
log.info("-----------------------------------")
log.info("STOP")
log.info("-----------------------------------")
if self.debug:
# Dump logged data to file
with open("data.txt", 'w') as data_file:
json.dump(data, data_file)
def _move(self, speed=0, steering=0, seconds=None):
"""Move robot."""
self.drive_queue.put((speed, steering))
if seconds is not None:
time.sleep(seconds)
self.drive_queue.put((0, 0))
self.drive_queue.join()
def move_forward(self, seconds=None):
"""Move robot forward."""
self._move(speed=SPEED_MAX, steering=0, seconds=seconds)
def move_backward(self, seconds=None):
"""Move robot backward."""
self._move(speed=-SPEED_MAX, steering=0, seconds=seconds)
def rotate_left(self, seconds=None):
"""Rotate robot left."""
self._move(speed=0, steering=STEER_MAX, seconds=seconds)
def rotate_right(self, seconds=None):
"""Rotate robot right."""
self._move(speed=0, steering=-STEER_MAX, seconds=seconds)
def stop(self):
"""Stop robot (balancing will continue)."""
self._move(speed=0, steering=0)
while ts1.wait_for_bump() == False:
sleep(0.01)
speed += 10
steer_pair.on(steering=0, speed=speed)
sleep(0.01)
def decel():
global speed
global loop
while ts2.wait_for_bump() == False:
sleep(0.01)
speed -= 10
steer_pair.on(steering=0, speed=speed)
sleep(0.01)
while True:
text = 'speed=' + str(speed)
show_text(text)
if ts1.wait_for_bump() == True:
speed -= 10
steer_pair.on(steering=0, speed=speed)
else:
sleep(0.1)
if ts2.wait_for_bump() == True:
speed += 10
steer_pair.on(steering=0, speed=speed)
else:
sleep(0.1)
