Skip to content

Commit b5e658a

Browse files
WasabiFanWasabiFan
authored
Flexible relative movement and improved parameter handling (#523)
1 parent 86ba6c4 commit b5e658a

2 files changed

Lines changed: 175 additions & 83 deletions

File tree

‎ev3dev2/motor.py‎

Lines changed: 36 additions & 76 deletions
Original file line numberDiff line numberDiff line change
@@ -803,14 +803,14 @@ def wait(self, cond, timeout=None):
803803
self._poll.register(self._state, select.POLLPRI)
804804

805805
while True:
806+
if cond(self.state):
807+
return True
808+
806809
self._poll.poll(None if timeout is None else timeout)
807810

808811
if timeout is not None and time.time() >= tic + timeout / 1000:
809812
return False
810813

811-
if cond(self.state):
812-
return True
813-
814814
def wait_until_not_moving(self, timeout=None):
815815
"""
816816
Blocks until ``running`` is not in ``self.state`` or ``stalled`` is in
@@ -867,25 +867,15 @@ def _speed_native_units(self, speed, label=None):
867867

868868
return speed.to_native_units(self)
869869

870-
def _set_position_rotations(self, speed, rotations):
871-
872-
# +/- speed is used to control direction, rotations must be positive
873-
assert rotations >= 0, "rotations is {}, must be >= 0".format(rotations)
870+
def _set_rel_position_degrees_and_speed_sp(self, degrees, speed):
871+
degrees = degrees if speed >= 0 else -degrees
872+
speed = abs(speed)
874873

875-
if speed > 0:
876-
self.position_sp = self.position + int(round(rotations * self.count_per_rot))
877-
else:
878-
self.position_sp = self.position - int(round(rotations * self.count_per_rot))
879-
880-
def _set_position_degrees(self, speed, degrees):
881-
882-
# +/- speed is used to control direction, degrees must be positive
883-
assert degrees >= 0, "degrees is %s, must be >= 0" % degrees
874+
position_delta = int(round((degrees * self.count_per_rot)/360))
875+
speed_sp = int(round(speed))
884876

885-
if speed > 0:
886-
self.position_sp = self.position + int((degrees * self.count_per_rot)/360)
887-
else:
888-
self.position_sp = self.position - int((degrees * self.count_per_rot)/360)
877+
self.position_sp = position_delta
878+
self.speed_sp = speed_sp
889879

890880
def _set_brake(self, brake):
891881
if brake:
@@ -900,17 +890,13 @@ def on_for_rotations(self, speed, rotations, brake=True, block=True):
900890
``speed`` can be a percentage or a :class:`ev3dev2.motor.SpeedValue`
901891
object, enabling use of other units.
902892
"""
903-
speed = self._speed_native_units(speed)
904-
905-
if not speed or not rotations:
906-
log.warning("({}) Either speed ({}) or rotations ({}) is invalid, motor will not move" .format(self, speed, rotations))
907-
self._set_brake(brake)
908-
return
893+
if speed is None or rotations is None:
894+
raise ValueError("Either speed ({}) or rotations ({}) is None".format(self, speed, rotations))
909895

910-
self.speed_sp = int(round(speed))
911-
self._set_position_rotations(speed, rotations)
896+
speed_sp = self._speed_native_units(speed)
897+
self._set_rel_position_degrees_and_speed_sp(rotations * 360, speed_sp)
912898
self._set_brake(brake)
913-
self.run_to_abs_pos()
899+
self.run_to_rel_pos()
914900

915901
if block:
916902
self.wait_until('running', timeout=WAIT_RUNNING_TIMEOUT)
@@ -923,17 +909,13 @@ def on_for_degrees(self, speed, degrees, brake=True, block=True):
923909
``speed`` can be a percentage or a :class:`ev3dev2.motor.SpeedValue`
924910
object, enabling use of other units.
925911
"""
926-
speed = self._speed_native_units(speed)
927-
928-
if not speed or not degrees:
929-
log.warning("({}) Either speed ({}) or degrees ({}) is invalid, motor will not move".format(self, speed, degrees))
930-
self._set_brake(brake)
931-
return
912+
if speed is None or degrees is None:
913+
raise ValueError("Either speed ({}) or degrees ({}) is None".format(self, speed, degrees))
932914

933-
self.speed_sp = int(round(speed))
934-
self._set_position_degrees(speed, degrees)
915+
speed_sp = self._speed_native_units(speed)
916+
self._set_rel_position_degrees_and_speed_sp(degrees, speed_sp)
935917
self._set_brake(brake)
936-
self.run_to_abs_pos()
918+
self.run_to_rel_pos()
937919

938920
if block:
939921
self.wait_until('running', timeout=WAIT_RUNNING_TIMEOUT)
@@ -1748,9 +1730,6 @@ def _block(self):
17481730
def _unpack_speeds_to_native_units(self, left_speed, right_speed):
17491731
left_speed = self.left_motor._speed_native_units(left_speed, "left_speed")
17501732
right_speed = self.right_motor._speed_native_units(right_speed, "right_speed")
1751-
1752-
assert left_speed or right_speed,\
1753-
"Either left_speed or right_speed must be non-zero"
17541733

17551734
return (
17561735
left_speed,
@@ -1767,32 +1746,7 @@ def on_for_rotations(self, left_speed, right_speed, rotations, brake=True, block
17671746
``rotations`` while the motor on the inside will have its requested
17681747
distance calculated according to the expected turn.
17691748
"""
1770-
(left_speed_native_units, right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed)
1771-
1772-
# proof of the following distance calculation: consider the circle formed by each wheel's path
1773-
# v_l = d_l/t, v_r = d_r/t
1774-
# therefore, t = d_l/v_l = d_r/v_r
1775-
if left_speed_native_units > right_speed_native_units:
1776-
left_rotations = rotations
1777-
right_rotations = abs(float(right_speed_native_units / left_speed_native_units)) * rotations
1778-
else:
1779-
left_rotations = abs(float(left_speed_native_units / right_speed_native_units)) * rotations
1780-
right_rotations = rotations
1781-
1782-
# Set all parameters
1783-
self.left_motor.speed_sp = int(round(left_speed_native_units))
1784-
self.left_motor._set_position_rotations(left_speed_native_units, left_rotations)
1785-
self.left_motor._set_brake(brake)
1786-
self.right_motor.speed_sp = int(round(right_speed_native_units))
1787-
self.right_motor._set_position_rotations(right_speed_native_units, right_rotations)
1788-
self.right_motor._set_brake(brake)
1789-
1790-
# Start the motors
1791-
self.left_motor.run_to_abs_pos()
1792-
self.right_motor.run_to_abs_pos()
1793-
1794-
if block:
1795-
self._block()
1749+
MoveTank.on_for_degrees(self, left_speed, right_speed, rotations * 360, brake, block)
17961750

17971751
def on_for_degrees(self, left_speed, right_speed, degrees, brake=True, block=True):
17981752
"""
@@ -1806,24 +1760,30 @@ def on_for_degrees(self, left_speed, right_speed, degrees, brake=True, block=Tru
18061760
"""
18071761
(left_speed_native_units, right_speed_native_units) = self._unpack_speeds_to_native_units(left_speed, right_speed)
18081762

1809-
if left_speed_native_units > right_speed_native_units:
1763+
# proof of the following distance calculation: consider the circle formed by each wheel's path
1764+
# v_l = d_l/t, v_r = d_r/t
1765+
# therefore, t = d_l/v_l = d_r/v_r
1766+
1767+
if degrees == 0 or (left_speed_native_units == 0 and right_speed_native_units == 0):
1768+
left_degrees = degrees
1769+
right_degrees = degrees
1770+
# larger speed by magnitude is the "outer" wheel, and rotates the full "degrees"
1771+
elif abs(left_speed_native_units) > abs(right_speed_native_units):
18101772
left_degrees = degrees
1811-
right_degrees = float(right_speed / left_speed_native_units) * degrees
1773+
right_degrees = abs(right_speed_native_units / left_speed_native_units) * degrees
18121774
else:
1813-
left_degrees = float(left_speed_native_units / right_speed_native_units) * degrees
1775+
left_degrees = abs(left_speed_native_units / right_speed_native_units) * degrees
18141776
right_degrees = degrees
18151777

18161778
# Set all parameters
1817-
self.left_motor.speed_sp = int(round(left_speed_native_units))
1818-
self.left_motor._set_position_degrees(left_speed_native_units, left_degrees)
1779+
self.left_motor._set_rel_position_degrees_and_speed_sp(left_degrees, left_speed_native_units)
18191780
self.left_motor._set_brake(brake)
1820-
self.right_motor.speed_sp = int(round(right_speed_native_units))
1821-
self.right_motor._set_position_degrees(right_speed_native_units, right_degrees)
1781+
self.right_motor._set_rel_position_degrees_and_speed_sp(right_degrees, right_speed_native_units)
18221782
self.right_motor._set_brake(brake)
18231783

18241784
# Start the motors
1825-
self.left_motor.run_to_abs_pos()
1826-
self.right_motor.run_to_abs_pos()
1785+
self.left_motor.run_to_rel_pos()
1786+
self.right_motor.run_to_rel_pos()
18271787

18281788
if block:
18291789
self._block()

‎tests/api_tests.py‎

Lines changed: 139 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,11 @@
1111

1212
import ev3dev2
1313
from ev3dev2.sensor.lego import InfraredSensor
14-
from ev3dev2.motor import Motor, MediumMotor, MoveTank, MoveSteering, MoveJoystick, SpeedPercent, SpeedDPM, SpeedDPS, SpeedRPM, SpeedRPS, SpeedNativeUnits, OUTPUT_A, OUTPUT_B
14+
from ev3dev2.motor import \
15+
Motor, MediumMotor, LargeMotor, \
16+
MoveTank, MoveSteering, MoveJoystick, \
17+
SpeedPercent, SpeedDPM, SpeedDPS, SpeedRPM, SpeedRPS, SpeedNativeUnits, \
18+
OUTPUT_A, OUTPUT_B
1519

1620
ev3dev2.Device.DEVICE_ROOT_PATH = os.path.join(FAKE_SYS, 'arena')
1721

@@ -128,20 +132,133 @@ def test_medium_motor_write(self):
128132
m.speed_sp = 500
129133
self.assertEqual(m.speed_sp, 500)
130134

131-
def test_move_tank(self):
135+
def test_motor_on_for_degrees(self):
136+
clean_arena()
137+
populate_arena([('large_motor', 0, 'outA')])
138+
139+
m = LargeMotor()
140+
141+
# simple case
142+
m.on_for_degrees(75, 100)
143+
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
144+
self.assertEqual(m.position_sp, 100)
145+
146+
# various negative cases; values act like multiplication
147+
m.on_for_degrees(-75, 100)
148+
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
149+
self.assertEqual(m.position_sp, -100)
150+
151+
m.on_for_degrees(75, -100)
152+
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
153+
self.assertEqual(m.position_sp, -100)
154+
155+
m.on_for_degrees(-75, -100)
156+
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
157+
self.assertEqual(m.position_sp, 100)
158+
159+
# zero speed (on-device, this will return immediately due to reported stall)
160+
m.on_for_degrees(0, 100)
161+
self.assertEqual(m.speed_sp, 0)
162+
self.assertEqual(m.position_sp, 100)
163+
164+
# zero distance
165+
m.on_for_degrees(75, 0)
166+
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
167+
self.assertEqual(m.position_sp, 0)
168+
169+
# zero speed and distance
170+
m.on_for_degrees(0, 0)
171+
self.assertEqual(m.speed_sp, 0)
172+
self.assertEqual(m.position_sp, 0)
173+
174+
# None speed
175+
with self.assertRaises(ValueError):
176+
m.on_for_degrees(None, 100)
177+
178+
# None distance
179+
with self.assertRaises(ValueError):
180+
m.on_for_degrees(75, None)
181+
182+
def test_motor_on_for_rotations(self):
183+
clean_arena()
184+
populate_arena([('large_motor', 0, 'outA')])
185+
186+
m = LargeMotor()
187+
188+
# simple case
189+
m.on_for_rotations(75, 5)
190+
self.assertEqual(m.speed_sp, int(round(0.75 * 1050)))
191+
self.assertEqual(m.position_sp, 5 * 360)
192+
193+
# None speed
194+
with self.assertRaises(ValueError):
195+
m.on_for_rotations(None, 5)
196+
197+
# None distance
198+
with self.assertRaises(ValueError):
199+
m.on_for_rotations(75, None)
200+
201+
def test_move_tank_relative_distance(self):
132202
clean_arena()
133203
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
134204

135205
drive = MoveTank(OUTPUT_A, OUTPUT_B)
206+
207+
# simple case (degrees)
208+
drive.on_for_degrees(50, 25, 100)
209+
self.assertEqual(drive.left_motor.position_sp, 100)
210+
self.assertEqual(drive.left_motor.speed_sp, 0.50 * 1050)
211+
self.assertEqual(drive.right_motor.position_sp, 50)
212+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0.25 * 1050, delta=0.5)
213+
214+
# simple case (rotations, based on degrees)
136215
drive.on_for_rotations(50, 25, 10)
216+
self.assertEqual(drive.left_motor.position_sp, 10 * 360)
217+
self.assertEqual(drive.left_motor.speed_sp, 0.50 * 1050)
218+
self.assertEqual(drive.right_motor.position_sp, 5 * 360)
219+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0.25 * 1050, delta=0.5)
220+
221+
# negative distance
222+
drive.on_for_rotations(50, 25, -10)
223+
self.assertEqual(drive.left_motor.position_sp, -10 * 360)
224+
self.assertEqual(drive.left_motor.speed_sp, 0.50 * 1050)
225+
self.assertEqual(drive.right_motor.position_sp, -5 * 360)
226+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0.25 * 1050, delta=0.5)
227+
228+
# negative speed
229+
drive.on_for_rotations(-50, 25, 10)
230+
self.assertEqual(drive.left_motor.position_sp, -10 * 360)
231+
self.assertEqual(drive.left_motor.speed_sp, 0.50 * 1050)
232+
self.assertEqual(drive.right_motor.position_sp, 5 * 360)
233+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0.25 * 1050, delta=0.5)
137234

138-
self.assertEqual(drive.left_motor.position, 0)
235+
# negative distance and speed
236+
drive.on_for_rotations(-50, 25, -10)
139237
self.assertEqual(drive.left_motor.position_sp, 10 * 360)
140-
self.assertEqual(drive.left_motor.speed_sp, 1050 / 2)
238+
self.assertEqual(drive.left_motor.speed_sp, 0.50 * 1050)
239+
self.assertEqual(drive.right_motor.position_sp, -5 * 360)
240+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0.25 * 1050, delta=0.5)
141241

142-
self.assertEqual(drive.right_motor.position, 0)
143-
self.assertEqual(drive.right_motor.position_sp, 5 * 360)
144-
self.assertAlmostEqual(drive.right_motor.speed_sp, 1050 / 4, delta=0.5)
242+
# both speeds zero but nonzero distance
243+
drive.on_for_rotations(0, 0, 10)
244+
self.assertEqual(drive.left_motor.position_sp, 10 * 360)
245+
self.assertAlmostEqual(drive.left_motor.speed_sp, 0)
246+
self.assertEqual(drive.right_motor.position_sp, 10 * 360)
247+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0)
248+
249+
# zero distance
250+
drive.on_for_rotations(25, 50, 0)
251+
self.assertEqual(drive.left_motor.position_sp, 0)
252+
self.assertAlmostEqual(drive.left_motor.speed_sp, 0.25 * 1050, delta=0.5)
253+
self.assertEqual(drive.right_motor.position_sp, 0)
254+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0.50 * 1050)
255+
256+
# zero distance and zero speed
257+
drive.on_for_rotations(0, 0, 0)
258+
self.assertEqual(drive.left_motor.position_sp, 0)
259+
self.assertAlmostEqual(drive.left_motor.speed_sp, 0)
260+
self.assertEqual(drive.right_motor.position_sp, 0)
261+
self.assertAlmostEqual(drive.right_motor.speed_sp, 0)
145262

146263
def test_tank_units(self):
147264
clean_arena()
@@ -172,6 +289,21 @@ def test_steering_units(self):
172289
self.assertEqual(drive.right_motor.position, 0)
173290
self.assertEqual(drive.right_motor.position_sp, 5 * 360)
174291
self.assertEqual(drive.right_motor.speed_sp, 200)
292+
293+
def test_steering_large_value(self):
294+
clean_arena()
295+
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
296+
297+
drive = MoveSteering(OUTPUT_A, OUTPUT_B)
298+
drive.on_for_rotations(-100, SpeedDPS(400), 10)
299+
300+
self.assertEqual(drive.left_motor.position, 0)
301+
self.assertEqual(drive.left_motor.position_sp, -10 * 360)
302+
self.assertEqual(drive.left_motor.speed_sp, 400)
303+
304+
self.assertEqual(drive.right_motor.position, 0)
305+
self.assertEqual(drive.right_motor.position_sp, 10 * 360)
306+
self.assertEqual(drive.right_motor.speed_sp, 400)
175307

176308
def test_joystick_units(self):
177309
clean_arena()

0 commit comments

Comments
 (0)