Skip to content

Commit 96f77b4

Browse files
dwalton76dwalton76
authored
Added MoveJoystick (#398)
* Added MoveJoystick * remove some dead code * Clean up angle comparisons
1 parent 6bd6721 commit 96f77b4

File tree

3 files changed

+210
-319
lines changed

3 files changed

+210
-319
lines changed

‎ev3dev/control/rc_tank.py‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11

22
import logging
3-
from ev3dev.auto import InfraredSensor, MoveTank
3+
from ev3dev.motor import MoveTank
4+
from ev3dev.sensor.lego import InfraredSensor
45
from time import sleep
56

67
log = logging.getLogger(__name__)
@@ -36,7 +37,7 @@ def main(self):
3637

3738
try:
3839
while True:
39-
self.remote.process(self.channel)
40+
self.remote.process()
4041
sleep(0.01)
4142

4243
# Exit cleanly so that all motors are stopped

‎ev3dev/control/webserver.py‎

Lines changed: 7 additions & 310 deletions
Original file line numberDiff line numberDiff line change
@@ -1,305 +1,14 @@
11
#!/usr/bin/env python3
22

33
import logging
4-
import math
54
import os
65
import re
7-
import sys
8-
import time
9-
import ev3dev.auto
10-
from ev3dev.helper import LargeMotorPair, list_motors
6+
from ev3dev.motor import MoveJoystick, list_motors, LargeMotor
117
from http.server import BaseHTTPRequestHandler, HTTPServer
12-
from time import sleep
138

149
log = logging.getLogger(__name__)
1510

1611

17-
# ===============================================
18-
# "Joystick" code for the web interface for tanks
19-
# ===============================================
20-
def angle_to_speed_percentage(angle):
21-
"""
22-
(1, 1)
23-
. . . . . . .
24-
. | .
25-
. | .
26-
(0, 1) . | . (1, 0)
27-
. | .
28-
. | .
29-
. | .
30-
. | .
31-
. | .
32-
. | x-axis .
33-
(-1, 1) .---------------------------------------. (1, -1)
34-
. | .
35-
. | .
36-
. | .
37-
. | y-axis .
38-
. | .
39-
(0, -1) . | . (-1, 0)
40-
. | .
41-
. | .
42-
. . . . . . .
43-
(-1, -1)
44-
45-
46-
The joystick is a circle within a circle where the (x, y) coordinates
47-
of the joystick form an angle with the x-axis. Our job is to translate
48-
this angle into the percentage of power that should be sent to each motor.
49-
For instance if the joystick is moved all the way to the top of the circle
50-
we want both motors to move forward with 100% power...that is represented
51-
above by (1, 1). If the joystick is moved all the way to the right side of
52-
the circle we want to rotate clockwise so we move the left motor forward 100%
53-
and the right motor backwards 100%...so (1, -1). If the joystick is at
54-
45 degrees then we move apply (1, 0) to move the left motor forward 100% and
55-
the right motor stays still.
56-
57-
The 8 points shown above are pretty easy. For the points in between those 8
58-
we do some math to figure out what the percentages should be. Take 11.25 degrees
59-
for example. We look at how the motors transition from 0 degrees to 45 degrees:
60-
- the left motor is 1 so that is easy
61-
- the right motor moves from -1 to 0
62-
63-
We determine how far we are between 0 and 45 degrees (11.25 is 25% of 45) so we
64-
know that the right motor should be 25% of the way from -1 to 0...so -0.75 is the
65-
percentage for the right motor at 11.25 degrees.
66-
"""
67-
68-
if angle >= 0 and angle <= 45:
69-
70-
# left motor stays at 1
71-
left_speed_percentage = 1
72-
73-
# right motor transitions from -1 to 0
74-
right_speed_percentage = -1 + (angle/45.0)
75-
76-
elif angle > 45 and angle <= 90:
77-
78-
# left motor stays at 1
79-
left_speed_percentage = 1
80-
81-
# right motor transitions from 0 to 1
82-
percentage_from_45_to_90 = (angle - 45) / 45.0
83-
right_speed_percentage = percentage_from_45_to_90
84-
85-
elif angle > 90 and angle <= 135:
86-
87-
# left motor transitions from 1 to 0
88-
percentage_from_90_to_135 = (angle - 90) / 45.0
89-
left_speed_percentage = 1 - percentage_from_90_to_135
90-
91-
# right motor stays at 1
92-
right_speed_percentage = 1
93-
94-
elif angle > 135 and angle <= 180:
95-
96-
# left motor transitions from 0 to -1
97-
percentage_from_135_to_180 = (angle - 135) / 45.0
98-
left_speed_percentage = -1 * percentage_from_135_to_180
99-
100-
# right motor stays at 1
101-
right_speed_percentage = 1
102-
103-
elif angle > 180 and angle <= 225:
104-
105-
# left motor transitions from -1 to 0
106-
percentage_from_180_to_225 = (angle - 180) / 45.0
107-
left_speed_percentage = -1 + percentage_from_180_to_225
108-
109-
# right motor transitions from 1 to -1
110-
# right motor transitions from 1 to 0 between 180 and 202.5
111-
if angle < 202.5:
112-
percentage_from_180_to_202 = (angle - 180) / 22.5
113-
right_speed_percentage = 1 - percentage_from_180_to_202
114-
115-
# right motor is 0 at 202.5
116-
elif angle == 202.5:
117-
right_speed_percentage = 0
118-
119-
# right motor transitions from 0 to -1 between 202.5 and 225
120-
else:
121-
percentage_from_202_to_225 = (angle - 202.5) / 22.5
122-
right_speed_percentage = -1 * percentage_from_202_to_225
123-
124-
elif angle > 225 and angle <= 270:
125-
126-
# left motor transitions from 0 to -1
127-
percentage_from_225_to_270 = (angle - 225) / 45.0
128-
left_speed_percentage = -1 * percentage_from_225_to_270
129-
130-
# right motor stays at -1
131-
right_speed_percentage = -1
132-
133-
elif angle > 270 and angle <= 315:
134-
135-
# left motor stays at -1
136-
left_speed_percentage = -1
137-
138-
# right motor transitions from -1 to 0
139-
percentage_from_270_to_315 = (angle - 270) / 45.0
140-
right_speed_percentage = -1 + percentage_from_270_to_315
141-
142-
elif angle > 315 and angle <= 360:
143-
144-
# left motor transitions from -1 to 1
145-
# left motor transitions from -1 to 0 between 315 and 337.5
146-
if angle < 337.5:
147-
percentage_from_315_to_337 = (angle - 315) / 22.5
148-
left_speed_percentage = (1 - percentage_from_315_to_337) * -1
149-
150-
# left motor is 0 at 337.5
151-
elif angle == 337.5:
152-
left_speed_percentage = 0
153-
154-
# left motor transitions from 0 to 1 between 337.5 and 360
155-
elif angle > 337.5:
156-
percentage_from_337_to_360 = (angle - 337.5) / 22.5
157-
left_speed_percentage = percentage_from_337_to_360
158-
159-
# right motor transitions from 0 to -1
160-
percentage_from_315_to_360 = (angle - 315) / 45.0
161-
right_speed_percentage = -1 * percentage_from_315_to_360
162-
163-
else:
164-
raise Exception('You created a circle with more than 360 degrees (%s)...that is quite the trick' % angle)
165-
166-
return (left_speed_percentage, right_speed_percentage)
167-
168-
169-
def xy_to_speed(x, y, max_speed, radius=100.0):
170-
"""
171-
Convert x,y joystick coordinates to left/right motor speed
172-
"""
173-
174-
vector_length = math.hypot(x, y)
175-
angle = math.degrees(math.atan2(y, x))
176-
177-
if angle < 0:
178-
angle += 360
179-
180-
# Should not happen but can happen (just by a hair) due to floating point math
181-
if vector_length > radius:
182-
vector_length = radius
183-
184-
# print "radius : %s" % radius
185-
# print "angle : %s" % angle
186-
# print "vector length : %s" % vector_length
187-
188-
(left_speed_percentage, right_speed_percentage) = angle_to_speed_percentage(angle)
189-
# print "init left_speed_percentage: %s" % left_speed_percentage
190-
# print "init right_speed_percentage: %s" % right_speed_percentage
191-
192-
# scale the speed percentages based on vector_length vs. radius
193-
left_speed_percentage = (left_speed_percentage * vector_length) / radius
194-
right_speed_percentage = (right_speed_percentage * vector_length) / radius
195-
# print "final left_speed_percentage: %s" % left_speed_percentage
196-
# print "final right_speed_percentage: %s" % right_speed_percentage
197-
198-
# calculate the motor speeds based on speed percentages and max_speed of the motors
199-
left_speed = round(left_speed_percentage * max_speed)
200-
right_speed = round(right_speed_percentage * max_speed)
201-
202-
# safety net
203-
if left_speed > max_speed:
204-
left_speed = max_speed
205-
206-
if right_speed > max_speed:
207-
right_speed = max_speed
208-
209-
return (left_speed, right_speed)
210-
211-
212-
def test_xy_to_speed():
213-
"""
214-
Used to test changes to xy_to_speed() and angle_to_speed_percentage()
215-
"""
216-
217-
# Move straight forward
218-
assert xy_to_speed(0, 100, 400) == (400, 400), "FAILED"
219-
220-
# Spin clockwise
221-
assert xy_to_speed(100, 0, 400) == (400, -400), "FAILED"
222-
223-
# Spin counter clockwise
224-
assert xy_to_speed(-100, 0, 400) == (-400, 400), "FAILED"
225-
226-
# Move straight back
227-
assert xy_to_speed(0, -100, 400) == (-400, -400), "FAILED"
228-
229-
# Test vector length to power percentages
230-
# Move straight forward, 1/2 power
231-
assert xy_to_speed(0, 50, 400) == (200, 200), "FAILED"
232-
233-
# Test motor max_speed
234-
# Move straight forward, 1/2 power with lower max_speed
235-
assert xy_to_speed(0, 50, 200) == (100, 100), "FAILED"
236-
237-
# http://www.pagetutor.com/trigcalc/trig.html
238-
239-
# top right quadrant
240-
# ==================
241-
# 0 -> 45 degrees
242-
assert xy_to_speed(98.07852804032305, 19.509032201612825, 400) == (400, -300), "FAILED" # 11.25 degrees
243-
assert xy_to_speed(92.38795325112868, 38.26834323650898, 400) == (400, -200), "FAILED" # 22.5 degrees
244-
assert xy_to_speed(83.14696123025452, 55.557023301960214, 400) == (400, -100), "FAILED" # 33.75 degrees
245-
246-
# 45 degrees, only left motor should turn
247-
assert xy_to_speed(70.71068, 70.71068, 400) == (400, 0), "FAILED"
248-
249-
# 45 -> 90 degrees
250-
assert xy_to_speed(55.55702330196023, 83.14696123025452, 400) == (400, 100), "FAILED" # 56.25 degrees
251-
assert xy_to_speed(38.26834323650898, 92.38795325112868, 400) == (400, 200), "FAILED" # 67.5 degrees
252-
assert xy_to_speed(19.509032201612833, 98.07852804032305, 400) == (400, 300), "FAILED" # 78.75 degrees
253-
254-
255-
# top left quadrant
256-
# =================
257-
# 90 -> 135 degrees
258-
assert xy_to_speed(-19.509032201612833, 98.07852804032305, 400) == (300, 400), "FAILED"
259-
assert xy_to_speed(-38.26834323650898, 92.38795325112868, 400) == (200, 400), "FAILED"
260-
assert xy_to_speed(-55.55702330196023, 83.14696123025452, 400) == (100, 400), "FAILED"
261-
262-
# 135 degrees, only right motor should turn
263-
assert xy_to_speed(-70.71068, 70.71068, 400) == (0, 400), "FAILED"
264-
265-
# 135 -> 180 degrees
266-
assert xy_to_speed(-83.14696123025452, 55.55702330196023, 400) == (-100, 400), "FAILED"
267-
assert xy_to_speed(-92.38795325112868, 38.26834323650898, 400) == (-200, 400), "FAILED"
268-
assert xy_to_speed(-98.07852804032305, 19.509032201612833, 400) == (-300, 400), "FAILED"
269-
270-
271-
# bottom left quadrant
272-
# ====================
273-
# 180 -> 225 degrees
274-
assert xy_to_speed(-98.07852804032305, -19.509032201612833, 400) == (-300, 200), "FAILED"
275-
assert xy_to_speed(-92.38795325112868, -38.26834323650898, 400) == (-200, 0), "FAILED"
276-
assert xy_to_speed(-83.14696123025452, -55.55702330196023, 400) == (-100, -200), "FAILED"
277-
278-
# 225 degrees, only right motor should turn (backwards)
279-
assert xy_to_speed(-70.71068, -70.71068, 400) == (0, -400), "FAILED"
280-
281-
# 225 -> 270 degrees
282-
assert xy_to_speed(-55.55702330196023, -83.14696123025452, 400) == (-100, -400), "FAILED"
283-
assert xy_to_speed(-38.26834323650898, -92.38795325112868, 400) == (-200, -400), "FAILED"
284-
assert xy_to_speed(-19.509032201612833, -98.07852804032305, 400) == (-300, -400), "FAILED"
285-
286-
287-
# bottom right quadrant
288-
# =====================
289-
# 270 -> 315 degrees
290-
assert xy_to_speed(19.509032201612833, -98.07852804032305, 400) == (-400, -300), "FAILED"
291-
assert xy_to_speed(38.26834323650898, -92.38795325112868, 400) == (-400, -200), "FAILED"
292-
assert xy_to_speed(55.55702330196023, -83.14696123025452, 400) == (-400, -100), "FAILED"
293-
294-
# 315 degrees, only left motor should turn (backwards)
295-
assert xy_to_speed(70.71068, -70.71068, 400) == (-400, 0), "FAILED"
296-
297-
# 315 -> 360 degrees
298-
assert xy_to_speed(83.14696123025452, -55.557023301960214, 400) == (-200, -100), "FAILED"
299-
assert xy_to_speed(92.38795325112868, -38.26834323650898, 400) == (0, -200), "FAILED"
300-
assert xy_to_speed(98.07852804032305, -19.509032201612825, 400) == (200, -300), "FAILED"
301-
302-
30312
# ==================
30413
# Web Server classes
30514
# ==================
@@ -372,7 +81,7 @@ def log_message(self, format, *args):
37281
max_move_xy_seq = 0
37382
motor_max_speed = None
37483
medium_motor_max_speed = None
375-
joystick_enaged = False
84+
joystick_engaged = False
37685

37786
class TankWebHandler(RobotWebHandler):
37887

@@ -426,8 +135,6 @@ def do_GET(self):
426135
We can also ignore any move-xy requests that show up late by tracking the
427136
max seq for any move-xy we service.
428137
'''
429-
# dwalton - fix this
430-
431138
path = self.path.split('/')
432139
seq = int(path[1])
433140
action = path[2]
@@ -502,19 +209,9 @@ def do_GET(self):
502209

503210
if joystick_engaged:
504211
if seq > max_move_xy_seq:
505-
(left_speed, right_speed) = xy_to_speed(x, y, motor_max_speed)
506-
log.debug("seq %d: (x, y) %4d, %4d -> speed %d %d" % (seq, x, y, left_speed, right_speed))
212+
self.robot.on(x, y, motor_max_speed)
507213
max_move_xy_seq = seq
508-
509-
if left_speed == 0:
510-
self.robot.left_motor.stop()
511-
else:
512-
self.robot.left_motor.run_forever(speed_sp=left_speed)
513-
514-
if right_speed == 0:
515-
self.robot.right_motor.stop()
516-
else:
517-
self.robot.right_motor.run_forever(speed_sp=right_speed)
214+
log.debug("seq %d: (x, y) (%4d, %4d)" % (seq, x, y))
518215
else:
519216
log.debug("seq %d: (x, y) %4d, %4d (ignore, max seq %d)" %
520217
(seq, x, y, max_move_xy_seq))
@@ -576,13 +273,13 @@ def run(self):
576273
motor.stop()
577274

578275

579-
class WebControlledTank(LargeMotorPair):
276+
class WebControlledTank(MoveJoystick):
580277
"""
581278
A tank that is controlled via a web browser
582279
"""
583280

584-
def __init__(self, left_motor, right_motor, polarity='normal', port_number=8000):
585-
LargeMotorPair.__init__(self, left_motor, right_motor, polarity)
281+
def __init__(self, left_motor, right_motor, port_number=8000, desc=None, motor_class=LargeMotor):
282+
MoveJoystick.__init__(self, left_motor, right_motor, desc, motor_class)
586283
self.www = RobotWebServer(self, TankWebHandler, port_number)
587284

588285
def main(self):

0 commit comments

Comments
 (0)