def driveStraightForDistance(self,
distance,
speedLeft=None,
speedRight=None,
stopAtEnd=True):
''' Drive for given distance.
Args:
distance: distance to drive in mm
speedLeft: speed in mm/s (left motor)
speedRight: speed in mm/s (right motor)
stopAtEnd (bool): brake at end of movement
'''
if speedLeft == None or speedLeft == 0:
speedLeft = self.defaultSpeed
if speedRight == None or speedRight == 0:
speedRight = self.defaultSpeed
degPerSecLeft = 360 * speedLeft / (pi * self.wheel_diameter)
degPerSecRight = 360 * speedRight / (pi * self.wheel_diameter)
driveTime = StopWatch()
driveTime.start()
self.leftDriveMotor.on(SpeedNativeUnits(degPerSecLeft), block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSecRight), block=False)
while driveTime.value_ms <= abs(distance / speedLeft) * 1000:
sleep(0.02)
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
def gotoXY(self, x, y, speed=30, block=True):
''' Move to absolute position (x,y).
moves axes simultaneously.
homeXY must be called first.
Args:
x: position of the x-axis (mm)
y: position of the y-axis (mm)
speed: the speed of the axes (mm/s)
block (bool): block until move is complete
'''
# start move to x position
self.xMotor.on_to_position(SpeedNativeUnits(speed * self.xRatio),
x * self.xRatio,
brake=False,
block=False)
# start move to y position
self.yMotor.on_to_position(SpeedNativeUnits(speed * self.yRatio),
y * self.yRatio,
brake=False,
block=False)
if block:
# sleep until either move is complete or motors stall
self.xMotor.wait_until_not_moving(self.defaultTimeout)
self.yMotor.wait_until_not_moving(self.defaultTimeout)
def line_follow(self, speed:int, distance_in, which_color_sensor='right', brake=True, block=True):
cs = self.choose_color_sensor(which_color_sensor)
integral = 0.0
last_error = 0.0
derivative = 0.0
kp = .5
ki = .1
kd = .1
target_light_intensity = self.line_square_intensity
speed_native_units = self.left_motor._speed_native_units(speed)
while self.follow_for_distance(distance_in, speed):
reflected_light_intensity = cs.reflected_light_intensity
error = target_light_intensity - reflected_light_intensity
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = (kp * error) + (ki * integral) + (kd * derivative)
left_speed = SpeedNativeUnits(speed_native_units - turn_native_units)
right_speed = SpeedNativeUnits(speed_native_units + turn_native_units)
self.debug("target_light_intensity: {}, error: {}, integral: {}, derivative: {}, last_error: {}, turn_native_units: {}, left_speed: {}, right_speed: {}".format(target_light_intensity, error, integral, derivative, last_error, turn_native_units, left_speed, right_speed))
self.on(left_speed, right_speed)
def on_for_distance(self, speed:int, distance_in:int, brake=True, block=True, use_gyro=False, target=0, which_gyro_sensor="right"):
"""
Drives for a certain distance
and has a toglable gyro feature
``speed`` can be a percentage or a :class:`ev3dev2.motor.SpeedValue`
object, enabling use of other units.
"""
# self.reset()
# values from ev3dev2 source
#kp = 11.3
#ki = 0.05
#kd = 3.2
kp = 11.3
ki = 2
kd = 3.2
distance_mm = self.in_to_mm(distance_in)
if use_gyro:
self.set_led('AMBER')
gyro = self.choose_gyro_sensor(which_gyro_sensor)
gyro.mode = gyro.MODE_GYRO_ANG
# there is currently a bug in gyro.reset() method so instead we just read the current
# angle and then compare to that
gyro.reset()
# reset_angle = gyro.angle
self.debug("GYRO ANGLE BEFORE LOOP: {}".format(gyro.angle))
# convert speed to a SpeedValue if not already
speed_native_units = self.left_motor._speed_native_units(speed)
integral = 0.0 # integral is the sum of all errors
last_error = 0.0 # last_error stores the last error through the while loop
derivative = 0.0 # derivative is the rate at which the error is changing
while self.follow_for_distance(distance_in, speed):
# error = gyro.angle - reset_angle # OLD CODE BEFORE GYRO.RESET FIXED
error = gyro.angle - target
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = (kp * error) + (ki * integral) + (kd * derivative)
left_speed = SpeedNativeUnits(speed_native_units - turn_native_units)
right_speed = SpeedNativeUnits(speed_native_units + turn_native_units)
self.debug("error: {}, integral: {}, derivative: {}, last_error: {},\
turn_native_units: {}, left_speed: {}, right_speed: {}".format\
(error, integral, derivative, last_error, turn_native_units, left_speed, right_speed))
self.on(left_speed, right_speed)
self.sleep_in_loop(0.01)
else:
super().on_for_distance(speed, distance_mm, brake, block)
self.set_led()
def pivot_gyro(self, speed, target_angle=0, sleep_time=0.01):
"""
Pivot Turn
``speed`` is the desired speed of the midpoint of the robot
``target_angle`` is the angle we want drive
``sleep_time`` is how many seconds we sleep on each pass through
the loop. This is to give the robot a chance to react
to the new motor settings. This should be something small such
as 0.01 (10ms).
Example:
.. code:: python
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent
from ev3dev2.sensor.lego import GyroSensor
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.gyro = GyroSensor()
try:
# Reset gyro sensor to zero
tank.calibrate_gyro()
# Pivot 30 degrees
tank.pivot_gyro(
speed=SpeedPercent(5),
target_angle(30)
)
except Exception:
tank.stop()
raise
"""
assert self.gyro, "GyroSensor must be defined"
speed_native_units = speed.to_native_units(self.left_motor)
target_reached = False
while not target_reached:
current_angle = self.gyro.angle
if (current_angle >= target_angle - 2
and current_angle <= target_angle + 2):
target_reached = True
self.stop()
elif (current_angle > target_angle):
left_speed = SpeedNativeUnits(-1 * speed_native_units)
right_speed = SpeedNativeUnits(speed_native_units)
else:
left_speed = SpeedNativeUnits(speed_native_units)
right_speed = SpeedNativeUnits(-1 * speed_native_units)
if sleep_time:
sleep(sleep_time)
self.on(left_speed, right_speed)
def follow_line_dual(self, kp, ki, kd, speed, sleep_time, follow_for,
**kwargs):
# PID line follower using both color sensors
# requires defintion of color sensors cs_l and cs_r
# dt = sleep_time
# RC = 1 / (2 * pi * LP_CUTOFF_FREQ)
e = e_prev = i = d_prev = 0.0
speed = speed_to_speedvalue(speed)
speed_native_units = speed.to_native_units(self.left_motor)
# t0 = time.clock()
# t = 0
max_speed = SPEED_MAX_NATIVE
while follow_for(self, **kwargs):
e = self.cs_l.reflected_light_intensity - self.cs_r.reflected_light_intensity
i += e
d = (e - e_prev)
# d = (e - e_prev) / dt
# d = d_prev + ((dt / (RC + dt)) * (d - d_prev))
# d_prev = d
u = (kp * e) + (ki * i) + (kd * d)
e_prev = e
# slewrate
# if (SLEW_RATE and t < 1):
# t = time.clock() - t0
# max_speed = SPEED_MAX_NATIVE * min(t * SLEW_RATE, 1.0)
# convert to native speed units (saturated)
speed_left = SpeedNativeUnits(
saturate(speed_native_units - u, max_speed))
speed_right = SpeedNativeUnits(
saturate(speed_native_units + u, max_speed))
# if sleep_time:
# time.sleep(sleep_time)
self.on(speed_left, speed_right)
self.stop()
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'),
('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(
SpeedPercent(35).to_native_units(m), 35 / 100 * m.max_speed)
self.assertEqual(SpeedDPS(300).to_native_units(m), 300)
self.assertEqual(SpeedNativeUnits(300).to_native_units(m), 300)
self.assertEqual(SpeedDPM(30000).to_native_units(m), (30000 / 60))
self.assertEqual(SpeedRPS(2).to_native_units(m), 360 * 2)
self.assertEqual(SpeedRPM(100).to_native_units(m), (360 * 100 / 60))
self.assertEqual(DistanceMillimeters(42).mm, 42)
self.assertEqual(DistanceCentimeters(42).mm, 420)
self.assertEqual(DistanceDecimeters(42).mm, 4200)
self.assertEqual(DistanceMeters(42).mm, 42000)
self.assertAlmostEqual(DistanceInches(42).mm, 1066.8)
self.assertAlmostEqual(DistanceFeet(42).mm, 12801.6)
self.assertAlmostEqual(DistanceYards(42).mm, 38404.8)
self.assertEqual(DistanceStuds(42).mm, 336)
def run(self):
print("Engine running!")
if self.handmax != 0:
self.calibrate()
# Change this function to suit your robot.
# The code below is for driving a simple tank.
while running:
# We have run out of records
if replay and record.empty():
continue
# Do normal replay
elif replay:
cur_record = record.get()
left_dc = cur_record[0][0]
left_speed = SpeedNativeUnits(cur_record[0][1])
right_dc = cur_record[1][0]
right_speed = SpeedNativeUnits(cur_record[1][1])
print(left_speed, left_dc, right_speed, right_dc)
self.tank.right_motor.on_to_position(right_speed, right_dc)
self.tank.left_motor.on_to_position(left_speed,
left_dc,
block=True)
# Run by controller
else:
right_dc = clamp(-speed - turn)
left_dc = clamp(-speed + turn)
self.tank.on(left_dc, right_dc)
if do_record:
record.put(([
self.tank.left_motor.position,
self.tank.left_motor.speed
], [
self.tank.right_motor.position,
self.tank.left_motor.speed
]))
#self.right_motor.on_fo(duty_cycle_sp=right_dc)
#self.left_motor.run_direct(duty_cycle_sp=left_dc)
if hand:
self.movehand()
hand = 0
self.movedrop(drop)
#self.right_motor.stop()
#self.left_motor.stop()
self.tank.stop()
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(SpeedPercent(35).get_speed_pct(m), 35)
self.assertEqual(SpeedDPS(300).get_speed_pct(m), 300 / 1050 * 100)
self.assertEqual(SpeedNativeUnits(300).get_speed_pct(m), 300 / 1050 * 100)
self.assertEqual(SpeedDPM(30000).get_speed_pct(m), (30000 / 60) / 1050 * 100)
self.assertEqual(SpeedRPS(2).get_speed_pct(m), 360 * 2 / 1050 * 100)
self.assertEqual(SpeedRPM(100).get_speed_pct(m), (360 * 100 / 60) / 1050 * 100)
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'),
('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(
SpeedPercent(35).to_native_units(m), 35 / 100 * m.max_speed)
self.assertEqual(SpeedDPS(300).to_native_units(m), 300)
self.assertEqual(SpeedNativeUnits(300).to_native_units(m), 300)
self.assertEqual(SpeedDPM(30000).to_native_units(m), (30000 / 60))
self.assertEqual(SpeedRPS(2).to_native_units(m), 360 * 2)
self.assertEqual(SpeedRPM(100).to_native_units(m), (360 * 100 / 60))
def follow_line(self,
kp,
ki,
kd,
speed,
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
prox_treshold=50,
**kwargs):
"""
PID line follower
``kp``, ``ki``, and ``kd`` are the PID constants.
``speed`` is the desired speed of the midpoint of the robot
``target_light_intensity`` is the reflected light intensity when the color sensor
is on the edge of the line. If this is None we assume that the color sensor
is on the edge of the line and will take a reading to set this variable.
``follow_left_edge`` determines if we follow the left or right edge of the line
``white`` is the reflected_light_intensity that is used to determine if we have
lost the line
``off_line_count_max`` is how many consecutive times through the loop the
reflected_light_intensity must be greater than ``white`` before we
declare the line lost and raise an exception
``sleep_time`` is how many seconds we sleep on each pass through
the loop. This is to give the robot a chance to react
to the new motor settings. This should be something small such
as 0.01 (10ms).
``follow_for`` is called to determine if we should keep following the
line or stop. This function will be passed ``self`` (the current
``MoveTank`` object). Current supported options are:
- ``follow_for_forever``
- ``follow_for_ms``
``**kwargs`` will be passed to the ``follow_for`` function
Example:
.. code:: python
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms
from ev3dev2.sensor.lego import ColorSensor
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.cs = ColorSensor()
try:
# Follow the line for 4500ms
tank.follow_line(
kp=11.3, ki=0.05, kd=3.2,
speed=SpeedPercent(30),
follow_for=follow_for_ms,
ms=4500
)
except Exception:
tank.stop()
raise
"""
assert self.cs, "ColorSensor must be defined"
if target_light_intensity is None:
target_light_intensity = self.cs.reflected_light_intensity
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = speed.to_native_units(self.left_motor)
print(speed_native_units)
MAX_SPEED = SpeedNativeUnits(self.max_speed)
button = Button()
while follow_for(self, **kwargs):
if "down" in button.buttons_pressed:
self.stop()
return
reflected_light_intensity = self.cs.reflected_light_intensity
error = reflected_light_intensity - target_light_intensity
integral = integral + error
derivative = error - last_error
print("target: {:^20} light: {:^20}, deri: {:^5}, integral: {:^5}".
format(target_light_intensity, reflected_light_intensity,
derivative, integral))
if derivative > 6: #or abs(integral) > 100:
self.stop()
print("Wrong way")
return
# self.on_for_degrees(SpeedPercent(-10), SpeedPercent(10), 100)
# integral = 0
# last_error = 0
# derivative = 0
# off_line_count = 0
# time.sleep(1)
# continue
if integral > 150:
self.stop()
print("Too high integral")
integral = -100.0
# We are correcting heading too much to the left, make correction to right
self.on_for_degrees(SpeedPercent(30), SpeedPercent(-30), 150)
self.on_until_target(target_light_intensity)
continue
last_error = error
turn_native_units = (kp * error) + (ki * integral) + (kd *
derivative)
if not follow_left_edge:
turn_native_units *= -1
left_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
right_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
# Is distance to wall too close?
#if self.us.proximity < prox_treshold:
# print("Too close")
# self.stop()
# in_line = reflected_light_intensity >= white
# turn_help = self.get_next_turn()
# # If we are still in line, safe to turn straight away
# if in_line:
# self.on_for_degrees(turn_help[0], turn_help[1], turn_help[2])
# else:
# self.on_for_seconds(SpeedPercent(-10), SpeedPercent(-10), 1)
# self.on_for_degrees(turn_help[0], turn_help[1], turn_help[2])
# # Reset values for new straigth line
# integral = 0
# last_error = 0
# derivative = 0
# off_line_count = 0
# time.sleep(2)
# continue
# Have we lost the line?
if reflected_light_intensity >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
self.stop()
print("Lost line")
raise LineFollowErrorLostLine("we lost the line")
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
self.on(left_speed, right_speed)
self.stop()
tank_pair = MoveTank(OUTPUT_B, OUTPUT_C)
leftMotor = ev3dev2.motor.Motor(OUTPUT_B)
rightMotor = ev3dev2.motor.Motor(OUTPUT_C)
# Connect an EV3 color sensor to any sensor port.
cl = ColorSensor()
while not btn.any():
leftSpeeds=[]
rightSpeeds=[]
for x in range(600):
# exit loop when any button pressed
# if we are over the black line (weak reflection)
rintensity=cl.reflected_light_intensity
lmotorpower=-((100/70)*(rintensity-70)+100)
rmotorpower=-(100-((100/70)*(rintensity-70)+100))
leftMotor.on(SpeedNativeUnits(lmotorpower))
rightMotor.on(SpeedNativeUnits(rmotorpower))
sleep(0.05)
leftSpeeds.append(leftMotor.speed)
rightSpeeds.append(rightMotor.speed)
"""if cl.reflected_light_intensity<30:
# medium turn right
leftMotor.on(SpeedNativeUnits(-100))
#leftMotor.speedNativeUnits(-500)
rightMotor.on(SpeedNativeUnits(0))
#leftMotor.speedNativeUnits(-500)
#rightMotor.speedNativeUnits(0)
leftSpeeds.append(leftMotor.speed)
rightSpeeds.append(rightMotor.speed)
def followLineDist(self,
distance,
sensor,
sideToFollow,
stopAtEnd,
speed,
gain_multiplier=1):
''' Drive forward following line.
Uses direct control of drive motors and PD control.
Args:
distance: distance to follow line in mm
sensor: colorsensor to use for line following
sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT
stopAtEnd (bool): Stop motors at end of line following
speed: speed in mm/s
gain_multiplier: multiplier for P and D gains
'''
degPerSec = 360 * speed / (pi * self.wheel_diameter)
propGain = 6.0 * gain_multiplier
derivGain = 6 * gain_multiplier
target = (self.blackThreshold + self.whiteThreshold) / 2
print("******* starting line following ********")
# print("error,Pterm,Dterm")
# initialize term for derivative calc
previousError = target - avgReflection(sensor, 2)
i = 0
driveTime = StopWatch()
driveTime.start()
while driveTime.value_ms <= abs(distance / speed) * 1000:
# calc error and proportional term
error = target - avgReflection(sensor, 2)
Pterm = propGain * error
# calc d(error)/d(t) and derivative term
d_error_dt = error - previousError
Dterm = derivGain * d_error_dt
previousError = error
if sideToFollow == LineEdge.RIGHT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
if sideToFollow == LineEdge.LEFT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
# eprint("line following complete")
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
# Play sound when we stop line following
sound.beep()
def follow_line(self,
kp,
ki,
kd,
side,
speed=SpeedPercent(50),
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
**kwargs):
self.cs = self.sLS_left
if target_light_intensity is None:
target_light_intensity = self.get_light_intensitiy(side)
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = -speed.to_native_units(self.motor_left)
MAX_SPEED = SpeedNativeUnits(self.max_speed / 2)
while follow_for(self, **kwargs):
if args.light_mode == 'ambient':
reflected_light_intensity = self.cs.ambient_light_intensity
else:
reflected_light_intensity = self.cs.reflected_light_intensity
error = target_light_intensity - reflected_light_intensity
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = -((kp * error) + (ki * integral) +
(kd * derivative))
if not follow_left_edge:
turn_native_units *= -1
#if error < last_error:
# speed_native_units -= 2
#elif error > last_error:
# speed_native_units += 5
right_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
left_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
print(turn_native_units, speed_native_units)
# DEBUG
#print('Error: %s' % error)
#print('reflected_light_intesity: : %s' % reflected_light_intensity)
#print('integral: %s' % integral)
#print('derivative: %s' % derivative)
#print('last_error: %s' % last_error)
#print('turn_native_units: %s' % turn_native_units)
#print('left_speed: %s' % left_speed)
#print('right_speed: %s' % right_speed)
if left_speed > MAX_SPEED:
print("%s: left_speed %s is greater than MAX_SPEED %s" %
(self, left_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
if right_speed > MAX_SPEED:
print("%s: right_speed %s is greater than MAX_SPEED %s" %
(self, right_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
# Have we lost the line?
if reflected_light_intensity >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
self.mMT.stop()
raise LineFollowErrorLostLine("we lost the line")
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
self.mMT.on(left_speed, right_speed)
self.mMT.stop()
def follow_gyro(self,
kp,
ki,
kd,
speed,
target_angle=0,
sleep_time=0.01,
follow_for=follow_for_forever,
**kwargs):
"""
PID line follower
``kp``, ``ki``, and ``kd`` are the PID constants.
``speed`` is the desired speed of the midpoint of the robot
``target_angle`` is the angle we want drive
``sleep_time`` is how many seconds we sleep on each pass through
the loop. This is to give the robot a chance to react
to the new motor settings. This should be something small such
as 0.01 (10ms).
``follow_for`` is called to determine if we should keep following the
line or stop. This function will be passed ``self`` (the current
``MoveWithGyroTank`` object). Current supported options are:
- ``follow_for_forever``
- ``follow_for_ms``
``**kwargs`` will be passed to the ``follow_for`` function
Example:
.. code:: python
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms
from ev3dev2.sensor.lego import GyroSensor
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.gyro = GyroSensor()
try:
# Follow the line for 4500ms
tank.calibrate_gyro()
tank.follow_gyro(
kp=11.3, ki=0.05, kd=3.2,
speed=SpeedPercent(30),
follow_for=follow_for_ms,
ms=4500
)
except Exception:
tank.stop()
raise
"""
assert self.gyro, "GyroSensor must be defined"
integral = 0.0
last_error = 0.0
derivative = 0.0
speed_native_units = speed.to_native_units(self.left_motor)
MAX_SPEED = SpeedNativeUnits(self.max_speed)
while follow_for(self, **kwargs):
current_angle = self.gyro.angle
error = current_angle - target_angle
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = (kp * error) + (ki * integral) + (kd *
derivative)
left_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
right_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
if left_speed > MAX_SPEED:
log.info("%s: left_speed %s is greater than MAX_SPEED %s" %
(self, left_speed, MAX_SPEED))
self.stop()
raise GyroFollowErrorTooFast(
"The robot is moving too fast to follow the angle")
if right_speed > MAX_SPEED:
log.info("%s: right_speed %s is greater than MAX_SPEED %s" %
(self, right_speed, MAX_SPEED))
self.stop()
raise GyroFollowErrorTooFast(
"The robot is moving too fast to follow the angle")
if sleep_time:
sleep(sleep_time)
self.on(left_speed, right_speed)
self.stop()
def follow_line(self,
kp,
ki,
kd,
side,
speed=SpeedPercent(50),
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
**kwargs):
target_light = self.sLS_left
other_light = self.sLS_right
if target_light_intensity is None:
time.sleep(1)
target_light_intensity = self.get_light_intensitiy(side)
buffed = target_light_intensity + 2.5
target_light_intensity_ls = other_light.reflected_light_intensity - 3
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = -speed.to_native_units(self.motor_left)
MAX_SPEED = SpeedNativeUnits(self.max_speed / 2)
switch_locked = False
while follow_for(self, **kwargs):
if args.light_mode == 'ambient':
target = target_light.ambient_light_intensity
other = other_light.ambient_light_intensity
else:
target = target_light.reflected_light_intensity
other = other_light.reflected_light_intensity
print("{:.3f}->{:.3f} | {:.3f}->{:.3f} | {:.3f} <-> {:.3f} | {}".
format(target, target_light_intensity, other,
target_light_intensity_ls, target_light_intensity,
target_light_intensity_ls, follow_left_edge))
if other < buffed and not switch_locked:
follow_left_edge = False
target_light = self.sLS_right
other_light = self.sLS_left
_tmp = target_light_intensity
target_light_intensity = target_light_intensity_ls
target_light_intensity_ls = _tmp
else:
follow_left_edge = True
target_light = self.sLS_left
other_light = self.sLS_right
#if reflected_light_intensity > buffed*2 and follow_left_edge is False and reflected_light_intensity_ls > target_light_intensity_ls*2:
# follow_left_edge = True
# switch_locked = False
error = target_light_intensity - target
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = -((kp * error) + (ki * integral) +
(kd * derivative))
if not follow_left_edge:
turn_native_units *= -1
#if error < last_error:
# speed_native_units -= 2
#elif error > last_error:
# speed_native_units += 5
right_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
left_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
#print(turn_native_units, speed_native_units)
# DEBUG
#print('Error: %s' % error)
#print('reflected_light_intesity: : %s' % reflected_light_intensity)
#print('integral: %s' % integral)
#print('derivative: %s' % derivative)
#print('last_error: %s' % last_error)
#print('turn_native_units: %s' % turn_native_units)
#print('left_speed: %s' % left_speed)
#print('right_speed: %s' % right_speed)
if left_speed > MAX_SPEED:
print("%s: left_speed %s is greater than MAX_SPEED %s" %
(self, left_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
if right_speed > MAX_SPEED:
print("%s: right_speed %s is greater than MAX_SPEED %s" %
(self, right_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
# Have we lost the line?
if target >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
#self.mMT.stop()
#raise LineFollowErrorLostLine("we lost the line")
pass
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
#self.mMT.on(left_speed, right_speed)
self.mMT.stop()
def follow_line(self,
kp,
ki,
kd,
side,
speed,
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
**kwargs):
"""
PID line follower
``kp``, ``ki``, and ``kd`` are the PID constants.
``speed`` is the desired speed of the midpoint of the robot
``target_light_intensity`` is the reflected light intensity when the color sensor
is on the edge of the line. If this is None we assume that the color sensor
is on the edge of the line and will take a reading to set this variable.
``follow_left_edge`` determines if we follow the left or right edge of the line
``white`` is the reflected_light_intensity that is used to determine if we have
lost the line
``off_line_count_max`` is how many consecutive times through the loop the
reflected_light_intensity must be greater than ``white`` before we
declare the line lost and raise an exception
``sleep_time`` is how many seconds we sleep on each pass through
the loop. This is to give the robot a chance to react
to the new motor settings. This should be something small such
as 0.01 (10ms).
``follow_for`` is called to determine if we should keep following the
line or stop. This function will be passed ``self`` (the current
``MoveTank`` object). Current supported options are:
- ``follow_for_forever``
- ``follow_for_ms``
``**kwargs`` will be passed to the ``follow_for`` function
Example:
.. code:: python
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms
from ev3dev2.sensor.lego import ColorSensor
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.cs = ColorSensor()
try:
# Follow the line for 4500ms
tank.follow_line(
kp=11.3, ki=0.05, kd=3.2,
speed=SpeedPercent(30),
follow_for=follow_for_ms,
ms=4500
)
except Exception:
tank.stop()
raise
"""
#assert self.cs, "ColorSensor must be defined"
if target_light_intensity is None:
target_light_intensity = self.get_light_intensitiy(side)
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = speed.to_native_units(self.left_motor)
MAX_SPEED = SpeedNativeUnits(self.max_speed)
while follow_for(self, **kwargs):
reflected_light_intensity = self.cs.reflected_light_intensity
error = target_light_intensity - reflected_light_intensity
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = (kp * error) + (ki * integral) + (kd *
derivative)
if not follow_left_edge:
turn_native_units *= -1
left_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
right_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
if left_speed > MAX_SPEED:
print("%s: left_speed %s is greater than MAX_SPEED %s" %
(self, left_speed, MAX_SPEED))
self.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
if right_speed > MAX_SPEED:
print("%s: right_speed %s is greater than MAX_SPEED %s" %
(self, right_speed, MAX_SPEED))
self.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
# Have we lost the line?
if reflected_light_intensity >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
self.stop()
raise LineFollowErrorLostLine("we lost the line")
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
self.on(left_speed, right_speed)
self.stop()
