time.sleep(0.5)
mL.reset()
time.sleep(2)
mL.duty_cycle_sp = 0
mLDtyMax = 16
mL.stop_action = 'coast'
mL.polarity = 'inversed'
mL.position = 0
StallDetected = False
mLlastPos = 0
i = 0
# Turn motor on with run_direct and duty_cycle_sp of '0'
mL.run_direct()
def keyboardInput(name):
while (True):
global x # Declare x as global to force use of global 'x' in this function/thread
x = ord(sys.stdin.read(1))
time.sleep(0.05)
if __name__ == "__main__":
myThread = threading.Thread(target=keyboardInput, args=(1,), daemon=True)
myThread.start()
print ("Ready for keyboard commands...")
#### Main Loop
mShuttle = LargeMotor(OUTPUT_D)
time.sleep(0.5)
mShuttle.reset()
time.sleep(2)
mShuttle.duty_cycle_sp = 0
mShuttleDtyMax = 22
mShuttle.stop_action = 'coast'
mShuttle.polarity = 'inversed'
mShuttle.position = 0
prev_mShuttle_pos = 0
# Turn motor on with run_direct and duty_cycle_sp of '0'
mShuttle.run_direct()
print ("Ready for keyboard commands...")
while (True):
x = ord(sys.stdin.read(1))
if x == 120: # x key pushed - exit
mShuttle.duty_cycle_sp = 0
mShuttle.off(brake=False)
break
if x == 32: # space key pushed
mShuttle.duty_cycle_sp = 0
if x == 43: # + key pushed
if mShuttle.duty_cycle_sp < mShuttleDtyMax:
mShuttle.duty_cycle_sp = mShuttle.duty_cycle_sp + 2
if x == 45: # - key pushed
mLift.stop_action = 'coast'
mLift.polarity = 'inversed'
mLift.position = 0
mGrab = LargeMotor(OUTPUT_D)
time.sleep(0.5)
mGrab.reset()
time.sleep(2)
mGrab.duty_cycle_sp = 0
mGrabDtyMax = 36
mGrab.stop_action = 'coast'
mGrab.polarity = 'inversed'
mGrab.position = 0
# Turn motors on with run_direct and duty_cycle_sp of '0'
mLift.run_direct()
mGrab.run_direct()
print("Ready for keyboard commands...")
while (True):
x = ord(sys.stdin.read(1))
if x == 120: # x key pushed - exit
mLift.duty_cycle_sp = 0
mGrab.duty_cycle_sp = 0
mLift.off(brake=False)
mGrab.off(brake=False)
break
if x == 32: # space key pushed
mLift.duty_cycle_sp = 0
mGrab.duty_cycle_sp = 0
from ev3dev2.display import Display
from ev3dev2.motor import LargeMotor, SpeedPercent, OUTPUT_A, OUTPUT_D
from ev3dev2.sensor import INPUT_1
from ev3dev2.port import LegoPort
from ev3dev2.button import Button
# Brick Variables
left_motor = LargeMotor(OUTPUT_D)
right_motor = LargeMotor(OUTPUT_A)
# Start program
target_duty_cycle = 75
correction_factor = 0.3
left_motor.duty_cycle_sp = target_duty_cycle
right_motor.duty_cycle_sp = target_duty_cycle
left_motor.run_direct()
right_motor.run_direct()
for i in range(100):
left_degrees = left_motor.degrees
right_degrees = right_motor.degrees
steering = (left_degrees - right_degrees) * correction_factor
left_duty = (target_duty_cycle - steering/2.0)
if left_duty > 100:
left_duty = 100
elif left_duty < -100:
left_duty = -100
left_motor.duty_cycle_sp = left_duty
right_duty = (target_duty_cycle + steering/2.0)
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)
class EV3Controller:
def __init__(self):
WHEEL_DISTANCE = 85 # 115
self.LeftMotor = LargeMotor(OUTPUT_B)
self.RightMotor = LargeMotor(OUTPUT_C)
# Run mode
self.LeftMotor.run_direct()
self.RightMotor.run_direct()
# Stop mode
self.LeftMotor.stop_action = LargeMotor.STOP_ACTION_HOLD # HOLD
self.RightMotor.stop_action = LargeMotor.STOP_ACTION_HOLD # HOLD
self.LeftSensor = ColorSensor(INPUT_1)
self.RightSensor = ColorSensor(INPUT_4)
def DetectJunctionDouble(self, threshold=30):
# Both sensors see black?
if (self.LeftSensor.reflected_light_intensity < threshold
and self.RightSensor.reflected_light_intensity <
threshold): # 30 is "ok"
return True
return False
def SetDutycycle(self, DutycycleLeft, DutycycleRight):
self.LeftMotor.duty_cycle_sp = DutycycleLeft
self.RightMotor.duty_cycle_sp = DutycycleRight
# Control motor speed
self.LeftMotor.command = LargeMotor.COMMAND_RUN_DIRECT
self.RightMotor.command = LargeMotor.COMMAND_RUN_DIRECT
def DrivePos(self, pos, speed=30):
self.LeftMotor.position = 0
self.RightMotor.position = 0
self.SetDutycycle(speed, speed)
while ((abs(self.LeftMotor.position) < pos) &
(abs(self.RightMotor.position) < pos)):
pass
self.SetDutycycle(0, 0)
def StopMotors(self):
self.SetDutycycle(0, 0)
self.LeftMotor.command = LargeMotor.COMMAND_STOP
self.RightMotor.command = LargeMotor.COMMAND_STOP
def TurnOnSpotSensor(self, TurnChar, TurnSpeed=50, twist=0):
if TurnChar == 'r': # Turn right
self.SetDutycycle(TurnSpeed, -TurnSpeed)
while (self.LeftSensor.reflected_light_intensity > 15):
pass
while (self.RightSensor.reflected_light_intensity > 20):
pass
if (twist):
while (self.LeftSensor.reflected_light_intensity > 40):
pass
if TurnChar == 'l': # Turn left
self.SetDutycycle(-TurnSpeed, TurnSpeed)
while (self.RightSensor.reflected_light_intensity > 15):
pass
while (self.LeftSensor.reflected_light_intensity > 20):
pass
if (twist):
while (self.RightSensor.reflected_light_intensity > 40):
pass
self.StopMotors()
def BounceFollow(self, max_speed=50, speed_reduction=25):
# WHITE = 100
# BLACK = 0
LeftIntensity = self.LeftSensor.reflected_light_intensity
RightIntensity = self.RightSensor.reflected_light_intensity
# QUICK FIX - LAV OM
if (LeftIntensity > 60 and RightIntensity > 60):
LeftIntensity = 60
RightIntensity = 60
leftDutycycle = max_speed - (
1 - LeftIntensity /
(LeftIntensity + RightIntensity)) * speed_reduction
rightDutycycle = max_speed - (
1 - RightIntensity /
(LeftIntensity + RightIntensity)) * speed_reduction
self.SetDutycycle(leftDutycycle, rightDutycycle)
nowError = False
preError = False
error_cont = 0
error = 0
angle = 0 # (deg)
rate = 0 # (deg/s)
time.sleep(0.01)
media_angle = offset() # (deg)
motorLeft.reset()
motorRight.reset()
motorLeft.duty_cycle_sp = 0
motorRight.duty_cycle_sp = 0
motorLeft.run_direct()
motorRight.run_direct()
vel = 0
pos = 0
avance = 0
max_acel = 0
extra_pwr = 0
tiempo = time.time()
t1 = []
t2 = []
t3 = []
while True:
pos_rel = pos_rel + vel * dt * 0.002
