def __init__(self):
self.shutdown = False
self.flipper = LargeMotor(OUTPUT_A)
self.turntable = LargeMotor(OUTPUT_B)
self.colorarm = MediumMotor(OUTPUT_C)
self.color_sensor = ColorSensor()
self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
self.infrared_sensor = InfraredSensor()
self.init_motors()
self.state = ['U', 'D', 'F', 'L', 'B', 'R']
self.rgb_solver = None
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
filename_max_rgb = 'max_rgb.txt'
if os.path.exists(filename_max_rgb):
with open(filename_max_rgb, 'r') as fh:
for line in fh:
(color, value) = line.strip().split()
if color == 'red':
self.color_sensor.red_max = int(value)
log.info("red max is %d" % self.color_sensor.red_max)
elif color == 'green':
self.color_sensor.green_max = int(value)
log.info("green max is %d" %
self.color_sensor.green_max)
elif color == 'blue':
self.color_sensor.blue_max = int(value)
log.info("blue max is %d" % self.color_sensor.blue_max)
#
# The program may be stopped by pressing any button on the brick.
#
# This demonstrates usage of motors, sound, sensors, buttons, and leds.
from time import sleep
from random import choice, randint
from ev3dev.motor import OUTPUT_B, OUTPUT_C, LargeMotor
from ev3dev.sensor.lego import InfraredSensor, TouchSensor
from ev3dev.button import Button
from ev3dev.led import Leds
from ev3dev.sound import Sound
# Connect two large motors on output ports B and C:
motors = [LargeMotor(address) for address in (OUTPUT_B, OUTPUT_C)]
# Connect infrared and touch sensors.
ir = InfraredSensor()
ts = TouchSensor()
print('Robot Starting')
# We will need to check EV3 buttons state.
btn = Button()
def start():
"""
Start both motors. `run-direct` command will allow to vary motor
performance on the fly by adjusting `duty_cycle_sp` attribute.
class MindCuber(object):
scan_order = [
5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54,
51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43,
44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31
]
hold_cube_pos = 85
rotate_speed = 400
flip_speed = 300
flip_speed_push = 400
def __init__(self):
self.shutdown = False
self.flipper = LargeMotor(OUTPUT_A)
self.turntable = LargeMotor(OUTPUT_B)
self.colorarm = MediumMotor(OUTPUT_C)
self.color_sensor = ColorSensor()
self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
self.infrared_sensor = InfraredSensor()
self.init_motors()
self.state = ['U', 'D', 'F', 'L', 'B', 'R']
self.rgb_solver = None
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
filename_max_rgb = 'max_rgb.txt'
if os.path.exists(filename_max_rgb):
with open(filename_max_rgb, 'r') as fh:
for line in fh:
(color, value) = line.strip().split()
if color == 'red':
self.color_sensor.red_max = int(value)
log.info("red max is %d" % self.color_sensor.red_max)
elif color == 'green':
self.color_sensor.green_max = int(value)
log.info("green max is %d" %
self.color_sensor.green_max)
elif color == 'blue':
self.color_sensor.blue_max = int(value)
log.info("blue max is %d" % self.color_sensor.blue_max)
def init_motors(self):
for x in (self.flipper, self.turntable, self.colorarm):
if not x.connected:
log.error("%s is not connected" % x)
sys.exit(1)
x.reset()
log.info("Initialize flipper %s" % self.flipper)
self.flipper.on(SpeedDPS(-50), block=True)
self.flipper.off()
self.flipper.reset()
log.info("Initialize colorarm %s" % self.colorarm)
self.colorarm.on(SpeedDPS(500), block=True)
self.colorarm.off()
self.colorarm.reset()
log.info("Initialize turntable %s" % self.turntable)
self.turntable.off()
self.turntable.reset()
def shutdown_robot(self):
log.info('Shutting down')
self.shutdown = True
if self.rgb_solver:
self.rgb_solver.shutdown = True
for x in (self.flipper, self.turntable, self.colorarm):
# We are shutting down so do not 'hold' the motors
x.stop_action = 'brake'
x.off(False)
def signal_term_handler(self, signal, frame):
log.error('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.error('Caught SIGINT')
self.shutdown_robot()
def apply_transformation(self, transformation):
self.state = [self.state[t] for t in transformation]
def rotate_cube(self, direction, nb):
current_pos = self.turntable.position
final_pos = 135 * round(
(self.turntable.position + (270 * direction * nb)) / 135.0)
log.info(
"rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" %
(direction, nb, current_pos, final_pos))
if self.flipper.position > 35:
self.flipper_away()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
final_pos)
if nb >= 1:
for i in range(nb):
if direction > 0:
transformation = [0, 1, 5, 2, 3, 4]
else:
transformation = [0, 1, 3, 4, 5, 2]
self.apply_transformation(transformation)
def rotate_cube_1(self):
self.rotate_cube(1, 1)
def rotate_cube_2(self):
self.rotate_cube(1, 2)
def rotate_cube_3(self):
self.rotate_cube(-1, 1)
def rotate_cube_blocked(self, direction, nb):
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# OVERROTATE depends on lot on MindCuber.rotate_speed
current_pos = self.turntable.position
OVERROTATE = 18
final_pos = int(135 * round(
(current_pos + (270 * direction * nb)) / 135.0))
temp_pos = int(final_pos + (OVERROTATE * direction))
log.info(
"rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s"
% (direction, nb, current_pos, temp_pos, final_pos))
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
temp_pos)
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
final_pos)
def rotate_cube_blocked_1(self):
self.rotate_cube_blocked(1, 1)
def rotate_cube_blocked_2(self):
self.rotate_cube_blocked(1, 2)
def rotate_cube_blocked_3(self):
self.rotate_cube_blocked(-1, 1)
def flipper_hold_cube(self, speed=300):
current_position = self.flipper.position
# Push it forward so the cube is always in the same position
# when we start the flip
if (current_position <= MindCuber.hold_cube_pos - 10
or current_position >= MindCuber.hold_cube_pos + 10):
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed),
MindCuber.hold_cube_pos)
sleep(0.05)
def flipper_away(self, speed=300):
"""
Move the flipper arm out of the way
"""
log.info("flipper_away()")
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed), 0)
def flip(self):
"""
Motors will sometimes stall if you call on_to_position() multiple
times back to back on the same motor. To avoid this we call a 50ms
sleep in flipper_hold_cube() and after each on_to_position() below.
We have to sleep after the 2nd on_to_position() because sometimes
flip() is called back to back.
"""
log.info("flip()")
if self.shutdown:
return
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# Grab the cube and pull back
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 0
self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
sleep(0.05)
# At this point the cube is at an angle, push it forward to
# drop it back down in the turntable
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
MindCuber.hold_cube_pos)
sleep(0.05)
transformation = [2, 4, 1, 3, 0, 5]
self.apply_transformation(transformation)
def colorarm_middle(self):
log.info("colorarm_middle()")
self.colorarm.on_to_position(SpeedDPS(600), -750)
def colorarm_corner(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_corner(%d)" % square_index)
position_target = -580
if square_index == 1:
position_target -= 10
elif square_index == 3:
position_target -= 30
elif square_index == 5:
position_target -= 20
elif square_index == 7:
pass
else:
raise ScanError(
"colorarm_corner was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_edge(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_edge(%d)" % square_index)
position_target = -640
if square_index == 2:
position_target -= 20
elif square_index == 4:
position_target -= 40
elif square_index == 6:
position_target -= 20
elif square_index == 8:
pass
else:
raise ScanError(
"colorarm_edge was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_remove(self):
log.info("colorarm_remove()")
self.colorarm.on_to_position(SpeedDPS(600), 0)
def colorarm_remove_halfway(self):
log.info("colorarm_remove_halfway()")
self.colorarm.on_to_position(SpeedDPS(600), -400)
def scan_face(self, face_number):
log.info("scan_face() %d/6" % face_number)
if self.shutdown:
return
if self.flipper.position > 35:
self.flipper_away(100)
self.colorarm_middle()
self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb
self.k += 1
i = 1
target_pos = 115
self.colorarm_corner(i)
# The gear ratio is 3:1 so 1080 is one full rotation
self.turntable.reset()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
1080,
block=False)
self.turntable.wait_until('running')
while True:
# 135 is 1/8 of full rotation
if self.turntable.position >= target_pos:
current_color = self.color_sensor.rgb
self.colors[int(MindCuber.scan_order[self.k])] = current_color
i += 1
self.k += 1
if i == 9:
# Last face, move the color arm all the way out of the way
if face_number == 6:
self.colorarm_remove()
# Move the color arm far enough away so that the flipper
# arm doesn't hit it
else:
self.colorarm_remove_halfway()
break
elif i % 2:
self.colorarm_corner(i)
if i == 1:
target_pos = 115
elif i == 3:
target_pos = 380
else:
target_pos = i * 135
else:
self.colorarm_edge(i)
if i == 2:
target_pos = 220
elif i == 8:
target_pos = 1060
else:
target_pos = i * 135
if self.shutdown:
return
if i < 9:
raise ScanError('i is %d..should be 9' % i)
self.turntable.wait_until_not_moving()
self.turntable.off()
self.turntable.reset()
log.info("\n")
def scan(self):
log.info("scan()")
self.colors = {}
self.k = 0
self.scan_face(1)
self.flip()
self.scan_face(2)
self.flip()
self.scan_face(3)
self.rotate_cube(-1, 1)
self.flip()
self.scan_face(4)
self.rotate_cube(1, 1)
self.flip()
self.scan_face(5)
self.flip()
self.scan_face(6)
if self.shutdown:
return
log.info("RGB json:\n%s\n" % json.dumps(self.colors))
self.rgb_solver = RubiksColorSolverGeneric(3)
self.rgb_solver.enter_scan_data(self.colors)
self.rgb_solver.crunch_colors()
self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict()
log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba))
# This is only used if you want to rotate the cube so U is on top, F is
# in the front, etc. You would do this if you were troubleshooting color
# detection and you want to pause to compare the color pattern on the
# cube vs. what we think the color pattern is.
'''
log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier")
self.rotate_cube(-1, 1)
self.flip()
self.flipper_away()
self.rotate_cube(1, 1)
input('Paused')
'''
def move(self, face_down):
log.info("move() face_down %s" % face_down)
position = self.state.index(face_down)
actions = {
0: ["flip", "flip"],
1: [],
2: ["rotate_cube_2", "flip"],
3: ["rotate_cube_1", "flip"],
4: ["flip"],
5: ["rotate_cube_3", "flip"]
}.get(position, None)
for a in actions:
if self.shutdown:
break
getattr(self, a)()
def run_kociemba_actions(self, actions):
log.info('Action (kociemba): %s' % ' '.join(actions))
total_actions = len(actions)
for (i, a) in enumerate(actions):
if self.shutdown:
break
if a.endswith("'"):
face_down = list(a)[0]
rotation_dir = 1
elif a.endswith("2"):
face_down = list(a)[0]
rotation_dir = 2
else:
face_down = a
rotation_dir = 3
log.info("Move %d/%d: %s%s (a %s)" %
(i, total_actions, face_down, rotation_dir, pformat(a)))
self.move(face_down)
if rotation_dir == 1:
self.rotate_cube_blocked_1()
elif rotation_dir == 2:
self.rotate_cube_blocked_2()
elif rotation_dir == 3:
self.rotate_cube_blocked_3()
log.info("\n")
def resolve(self):
if self.shutdown:
return
cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))]
output = check_output(cmd).decode('ascii')
if 'ERROR' in output:
msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd),
output)
log.error(msg)
print(msg)
sys.exit(1)
actions = output.strip().split()
self.run_kociemba_actions(actions)
self.cube_done()
def cube_done(self):
self.flipper_away()
def wait_for_cube_insert(self):
rubiks_present = 0
rubiks_present_target = 10
log.info('wait for cube...to be inserted')
while True:
if self.shutdown:
break
dist = self.infrared_sensor.proximity
# It is odd but sometimes when the cube is inserted
# the IR sensor returns a value of 100...most of the
# time it is just a value less than 50
if dist < 50 or dist == 100:
rubiks_present += 1
log.info("wait for cube...distance %d, present for %d/%d" %
(dist, rubiks_present, rubiks_present_target))
else:
if rubiks_present:
log.info('wait for cube...cube removed (%d)' % dist)
rubiks_present = 0
if rubiks_present >= rubiks_present_target:
log.info('wait for cube...cube found and stable')
break
time.sleep(0.1)
#!/usr/bin/env python3 #!/usr/bin/env python3 from ev3dev.motor import LargeMotor, OUTPUT_A, OUTPUT_B, LargeMotor, MoveSteering from ev3dev.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM from time import sleep lm1 = LargeMotor(OUTPUT_A) lm2 = LargeMotor(OUTPUT_B) lm1.on_for_seconds(speed=50, seconds=3) lm2.on_for_seconds(speed=50, seconds=3) sleep(1) lm1.on_for_seconds(speed=50, seconds=3) sleep(1) lm2.on_for_seconds(speed=50, seconds=3) sleep(1) steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B, motor_class=LargeMotor) steer_pair.on_for_seconds(steering=-100, speed=50, seconds=2)
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
# 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_file = open(self.gyro._path + "/value0", "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)
class GyroBalancer(object):
"""
Base class for a robot that stands on two wheels and uses a gyro sensor.
Robot will keep its balance.
"""
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
# 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_file = open(self.gyro._path + "/value0", "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_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.")
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_offset = gyro_offset + self._fast_read(self.gyro_file)
time.sleep(0.01)
gyro_offset = gyro_offset / gyro_calibrate_count
# 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_readings = OrderedDict()
data['gyro_readings'] = gyro_readings
# 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_file)
# 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_readings[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
# 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)