Skip to content

Commit 33b1134

Browse files
dwalton76dwalton76
authored
PID Line follower (#630)
1 parent 0582aee commit 33b1134

File tree

5 files changed

+324
-14
lines changed

5 files changed

+324
-14
lines changed

‎debian/changelog‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
python-ev3dev2 (2.0.0~beta4) UNRELEASED; urgency=medium
22

33
[Daniel Walton]
4+
* PID line follower
45
* micropython Button support
56
* micropython Sound support
67
* micropython support for LED animations

‎ev3dev2/__init__.py‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -246,7 +246,7 @@ def _get_attribute(self, attribute, name):
246246
attribute.seek(0)
247247
return attribute, attribute.read().strip().decode()
248248
except Exception as ex:
249-
self._raise_friendly_access_error(ex, name)
249+
self._raise_friendly_access_error(ex, name, None)
250250

251251
def _set_attribute(self, attribute, name, value):
252252
"""Device attribute setter"""
@@ -261,10 +261,10 @@ def _set_attribute(self, attribute, name, value):
261261
attribute.write(value)
262262
attribute.flush()
263263
except Exception as ex:
264-
self._raise_friendly_access_error(ex, name)
264+
self._raise_friendly_access_error(ex, name, value)
265265
return attribute
266266

267-
def _raise_friendly_access_error(self, driver_error, attribute):
267+
def _raise_friendly_access_error(self, driver_error, attribute, value):
268268
if not isinstance(driver_error, OSError):
269269
raise driver_error
270270

@@ -275,10 +275,11 @@ def _raise_friendly_access_error(self, driver_error, attribute):
275275
try:
276276
max_speed = self.max_speed
277277
except (AttributeError, Exception):
278-
chain_exception(ValueError("The given speed value was out of range"), driver_error)
278+
chain_exception(ValueError("The given speed value {} was out of range".format(value)),
279+
driver_error)
279280
else:
280-
chain_exception(ValueError("The given speed value was out of range. Max speed: +/-" + str(max_speed)), driver_error)
281-
chain_exception(ValueError("One or more arguments were out of range or invalid"), driver_error)
281+
chain_exception(ValueError("The given speed value {} was out of range. Max speed: +/-{}".format(value, max_speed)), driver_error)
282+
chain_exception(ValueError("One or more arguments were out of range or invalid, value {}".format(value)), driver_error)
282283
elif driver_errorno == errno.ENODEV or driver_errorno == errno.ENOENT:
283284
# We will assume that a file-not-found error is the result of a disconnected device
284285
# rather than a library error. If that isn't the case, at a minimum the underlying

‎ev3dev2/motor.py‎

Lines changed: 185 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
from logging import getLogger
4141
from os.path import abspath
4242
from ev3dev2 import get_current_platform, Device, list_device_names
43+
from ev3dev2.stopwatch import StopWatch
4344

4445
log = getLogger(__name__)
4546

@@ -83,9 +84,24 @@ class SpeedValue(object):
8384
:class:`SpeedDPS`, and :class:`SpeedDPM`.
8485
"""
8586

87+
def __eq__(self, other):
88+
return self.to_native_units() == other.to_native_units()
89+
90+
def __ne__(self, other):
91+
return not self.__eq__(other)
92+
8693
def __lt__(self, other):
8794
return self.to_native_units() < other.to_native_units()
8895

96+
def __le__(self, other):
97+
return self.to_native_units() <= other.to_native_units()
98+
99+
def __gt__(self, other):
100+
return self.to_native_units() > other.to_native_units()
101+
102+
def __ge__(self, other):
103+
return self.to_native_units() >= other.to_native_units()
104+
89105
def __rmul__(self, other):
90106
return self.__mul__(other)
91107

@@ -124,13 +140,13 @@ def __init__(self, native_counts):
124140
self.native_counts = native_counts
125141

126142
def __str__(self):
127-
return str(self.native_counts) + " counts/sec"
143+
return "{:.2f}".format(self.native_counts) + " counts/sec"
128144

129145
def __mul__(self, other):
130146
assert isinstance(other, (float, int)), "{} can only be multiplied by an int or float".format(self)
131147
return SpeedNativeUnits(self.native_counts * other)
132148

133-
def to_native_units(self, motor):
149+
def to_native_units(self, motor=None):
134150
"""
135151
Return this SpeedNativeUnits as a number
136152
"""
@@ -1774,6 +1790,46 @@ def _block(self):
17741790
self.wait_until_not_moving()
17751791

17761792

1793+
# line follower classes
1794+
class LineFollowErrorLostLine(Exception):
1795+
"""
1796+
Raised when a line following robot has lost the line
1797+
"""
1798+
pass
1799+
1800+
1801+
class LineFollowErrorTooFast(Exception):
1802+
"""
1803+
Raised when a line following robot has been asked to follow
1804+
a line at an unrealistic speed
1805+
"""
1806+
pass
1807+
1808+
1809+
# line follower functions
1810+
def follow_for_forever(tank):
1811+
"""
1812+
``tank``: the MoveTank object that is following a line
1813+
"""
1814+
return True
1815+
1816+
1817+
def follow_for_ms(tank, ms):
1818+
"""
1819+
``tank``: the MoveTank object that is following a line
1820+
``ms`` : the number of milliseconds to follow the line
1821+
"""
1822+
if not hasattr(tank, 'stopwatch') or tank.stopwatch is None:
1823+
tank.stopwatch = StopWatch()
1824+
tank.stopwatch.start()
1825+
1826+
if tank.stopwatch.value_ms >= ms:
1827+
tank.stopwatch = None
1828+
return False
1829+
else:
1830+
return True
1831+
1832+
17771833
class MoveTank(MotorSet):
17781834
"""
17791835
Controls a pair of motors simultaneously, via individual speed setpoints for each motor.
@@ -1798,6 +1854,9 @@ def __init__(self, left_motor_port, right_motor_port, desc=None, motor_class=Lar
17981854
self.right_motor = self.motors[right_motor_port]
17991855
self.max_speed = self.left_motor.max_speed
18001856

1857+
# color sensor used by follow_line()
1858+
self.cs = None
1859+
18011860
def _unpack_speeds_to_native_units(self, left_speed, right_speed):
18021861
left_speed = self.left_motor._speed_native_units(left_speed, "left_speed")
18031862
right_speed = self.right_motor._speed_native_units(right_speed, "right_speed")
@@ -1921,6 +1980,125 @@ def on(self, left_speed, right_speed):
19211980
self.left_motor.run_forever()
19221981
self.right_motor.run_forever()
19231982

1983+
def follow_line(self,
1984+
kp, ki, kd,
1985+
speed,
1986+
target_light_intensity=None,
1987+
follow_left_edge=True,
1988+
white=60,
1989+
off_line_count_max=20,
1990+
sleep_time=0.01,
1991+
follow_for=follow_for_forever,
1992+
**kwargs
1993+
):
1994+
"""
1995+
PID line follower
1996+
1997+
``kp``, ``ki``, and ``kd`` are the PID constants.
1998+
1999+
``speed`` is the desired speed of the midpoint of the robot
2000+
2001+
``target_light_intensity`` is the reflected light intensity when the color sensor
2002+
is on the edge of the line. If this is None we assume that the color sensor
2003+
is on the edge of the line and will take a reading to set this variable.
2004+
2005+
``follow_left_edge`` determines if we follow the left or right edge of the line
2006+
2007+
``white`` is the reflected_light_intensity that is used to determine if we have
2008+
lost the line
2009+
2010+
``off_line_count_max`` is how many consecutive times through the loop the
2011+
reflected_light_intensity must be greater than ``white`` before we
2012+
declare the line lost and raise an exception
2013+
2014+
``sleep_time`` is how many seconds we sleep on each pass through
2015+
the loop. This is to give the robot a chance to react
2016+
to the new motor settings. This should be something small such
2017+
as 0.01 (10ms).
2018+
2019+
``follow_for`` is called to determine if we should keep following the
2020+
line or stop. This function will be passed ``self`` (the current
2021+
``MoveTank`` object). Current supported options are:
2022+
- ``follow_for_forever``
2023+
- ``follow_for_ms``
2024+
2025+
``**kwargs`` will be passed to the ``follow_for`` function
2026+
2027+
Example:
2028+
2029+
.. code:: python
2030+
2031+
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms
2032+
from ev3dev2.sensor.lego import ColorSensor
2033+
2034+
tank = MoveTank(OUTPUT_A, OUTPUT_B)
2035+
tank.cs = ColorSensor()
2036+
2037+
try:
2038+
# Follow the line for 4500ms
2039+
tank.follow_line(
2040+
kp=11.3, ki=0.05, kd=3.2,
2041+
speed=SpeedPercent(30),
2042+
follow_for=follow_for_ms,
2043+
ms=4500
2044+
)
2045+
except Exception:
2046+
tank.stop()
2047+
raise
2048+
"""
2049+
assert self.cs, "ColorSensor must be defined"
2050+
2051+
if target_light_intensity is None:
2052+
target_light_intensity = self.cs.reflected_light_intensity
2053+
2054+
integral = 0.0
2055+
last_error = 0.0
2056+
derivative = 0.0
2057+
off_line_count = 0
2058+
speed_native_units = speed.to_native_units(self.left_motor)
2059+
MAX_SPEED = SpeedNativeUnits(self.max_speed)
2060+
2061+
while follow_for(self, **kwargs):
2062+
reflected_light_intensity = self.cs.reflected_light_intensity
2063+
error = target_light_intensity - reflected_light_intensity
2064+
integral = integral + error
2065+
derivative = error - last_error
2066+
last_error = error
2067+
turn_native_units = (kp * error) + (ki * integral) + (kd * derivative)
2068+
2069+
if not follow_left_edge:
2070+
turn_native_units *= -1
2071+
2072+
left_speed = SpeedNativeUnits(speed_native_units - turn_native_units)
2073+
right_speed = SpeedNativeUnits(speed_native_units + turn_native_units)
2074+
2075+
if left_speed > MAX_SPEED:
2076+
log.info("%s: left_speed %s is greater than MAX_SPEED %s" % (self, left_speed, MAX_SPEED))
2077+
self.stop()
2078+
raise LineFollowErrorTooFast("The robot is moving too fast to follow the line")
2079+
2080+
if right_speed > MAX_SPEED:
2081+
log.info("%s: right_speed %s is greater than MAX_SPEED %s" % (self, right_speed, MAX_SPEED))
2082+
self.stop()
2083+
raise LineFollowErrorTooFast("The robot is moving too fast to follow the line")
2084+
2085+
# Have we lost the line?
2086+
if reflected_light_intensity >= white:
2087+
off_line_count += 1
2088+
2089+
if off_line_count >= off_line_count_max:
2090+
self.stop()
2091+
raise LineFollowErrorLostLine("we lost the line")
2092+
else:
2093+
off_line_count = 0
2094+
2095+
if sleep_time:
2096+
time.sleep(sleep_time)
2097+
2098+
self.on(left_speed, right_speed)
2099+
2100+
self.stop()
2101+
19242102

19252103
class MoveSteering(MoveTank):
19262104
"""
@@ -2224,7 +2402,7 @@ def odometry_coordinates_log(self):
22242402

22252403
def odometry_start(self, theta_degrees_start=90.0,
22262404
x_pos_start=0.0, y_pos_start=0.0,
2227-
SLEEP_TIME=0.005): # 5ms
2405+
sleep_time=0.005): # 5ms
22282406
"""
22292407
Ported from:
22302408
http://seattlerobotics.org/encoder/200610/Article3/IMU%20Odometry,%20by%20David%20Anderson.htm
@@ -2254,8 +2432,8 @@ def _odometry_monitor():
22542432

22552433
# Have we moved?
22562434
if not left_ticks and not right_ticks:
2257-
if SLEEP_TIME:
2258-
time.sleep(SLEEP_TIME)
2435+
if sleep_time:
2436+
time.sleep(sleep_time)
22592437
continue
22602438

22612439
# log.debug("%s: left_ticks %s (from %s to %s)" %
@@ -2288,8 +2466,8 @@ def _odometry_monitor():
22882466
self.x_pos_mm += mm * math.cos(self.theta)
22892467
self.y_pos_mm += mm * math.sin(self.theta)
22902468

2291-
if SLEEP_TIME:
2292-
time.sleep(SLEEP_TIME)
2469+
if sleep_time:
2470+
time.sleep(sleep_time)
22932471

22942472
self.odometry_thread_id = None
22952473

0 commit comments

Comments
 (0)