Skip to content

Commit 7d682a8

Browse files
hwillemenhwillemen
committed
Diagnostics script example
see pupdevice.rst for description. A lot of dynamic features for more advanced programming
1 parent 9b93bf5 commit 7d682a8

File tree

2 files changed

+364
-0
lines changed

2 files changed

+364
-0
lines changed

‎doc/main/iodevices/pupdevice.rst‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,3 +23,16 @@ Detecting devices
2323

2424
.. literalinclude::
2525
../../../examples/pup/iodevices_pupdevice/port_info.py
26+
27+
Diagnostics: bringing examples together.
28+
******************************
29+
30+
``HubInit()`` automatically connects to any of the known hubs and reports its features in ``hub_info``
31+
The init loops over all the available ports and ``ConnectToDevice()`` connects with available devices.
32+
The remote, if there is one, controls 2 variables; in range(-100,100), the other in range(0,100).
33+
These could be used to control 1 or more motors, lights, etc.
34+
On ``DIAGNOSTICS_PERIOD`` 4 different sensors values are read if available together with hub voltage and current.
35+
Also ``ch1_val`` is applied as dc() value on motors if there are any.
36+
37+
.. literalinclude::
38+
../../../examples/pup/iodevices_pupdevice/diagnostics.py

‎examples/pup/iodevices_pupdevice/diagnostics.py‎

Lines changed: 351 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,351 @@
1+
from pybricks.geometry import Axis
2+
from pybricks.iodevices import PUPDevice
3+
from pybricks.pupdevices import *
4+
from pybricks.parameters import Port, Button, Color
5+
from pybricks.tools import wait, StopWatch
6+
from uerrno import ENODEV, ETIMEDOUT
7+
8+
# 1: Determine the type of hub
9+
# -------------------------------------------------------------------
10+
# For Hubs with IMU: how is it mounted?
11+
# https://docs.pybricks.com/en/latest/geometry.html#fig-imuexamples
12+
def HubInit(mounted_top=Axis.Z, mounted_front=Axis.X):
13+
# Returns (str hub_object, dict hub_info)
14+
hub = None
15+
hub_info = dict()
16+
try:
17+
from pybricks.hubs import MoveHub
18+
hub = MoveHub()
19+
hub_info.update(hub_type="MoveHub")
20+
except ImportError:
21+
pass
22+
try:
23+
from pybricks.hubs import CityHub
24+
hub = CityHub()
25+
hub_info.update(hub_type="CityHub")
26+
except ImportError:
27+
pass
28+
try:
29+
from pybricks.hubs import TechnicHub
30+
hub = TechnicHub(top_side=mounted_top, front_side=mounted_front)
31+
hub_info.update(hub_type="TechnicHub")
32+
except ImportError:
33+
pass
34+
try: # InventorHub is the same as PrimeHub, so treat the same.
35+
from pybricks.hubs import PrimeHub
36+
hub = PrimeHub(top_side=mounted_top, front_side=mounted_front)
37+
hub_info.update(hub_type="PrimeHub")
38+
except ImportError:
39+
pass
40+
try:
41+
from pybricks.hubs import EssentialHub
42+
hub = EssentialHub(top_side=mounted_top, front_side=mounted_front)
43+
hub_info.update(hub_type="EssentialHub")
44+
except ImportError:
45+
pass
46+
47+
if hub is None:
48+
raise Exception("No valid Hub found")
49+
50+
print("Hub:\n Name:\t", hub.system.name(),":",hub_info['hub_type'])
51+
print(" Reset reason: ", hub.system.reset_reason())
52+
print(" voltage: ", hub.battery.voltage(), "mV")
53+
print(" current: ", hub.battery.current(), "mA")
54+
55+
# Check for IMU and Matrix dislay:
56+
try: # to init IMU
57+
from pybricks.parameters import Side
58+
hub_info.update(init_position=hub.imu.up())
59+
print(" facing: ", hub_info['init_position'])
60+
hub_info.update(has_imu=True)
61+
except AttributeError:
62+
hub_info.update(has_imu=False)
63+
pass
64+
try: # to init Matrix
65+
from pybricks.parameters import Icon
66+
hub.display.icon(Icon.UP / 2)
67+
hub.display.orientation(up_side)
68+
print(" with display matrix.")
69+
hub_info.update(has_matrix=True)
70+
except ImportError or AttributeError:
71+
hub_info.update(has_matrix=False)
72+
pass
73+
74+
return (hub, hub_info)
75+
76+
# 2: Diagnose connected devices
77+
# -------------------------------------------------------------------
78+
# Dictionary of device identifiers along with their name.
79+
# Also check https://github.com/pybricks/technical-info/blob/master/assigned-numbers.md#io-device-type-ids
80+
# for retired/internal
81+
device_names = {
82+
1: "Wedo 2.0 Medium Motor", #DCMotor, analog ID
83+
2: "PUP Train Motor", #DCMotor, analog ID
84+
8: "PUP Light", #Light, analog ID
85+
34: "Wedo 2.0 Tilt Sensor",
86+
35: "Wedo 2.0 Infrared Sensor", # aka WeDo 2.0 Motion Sensor
87+
37: "BOOST Color Distance Sensor",
88+
38: "BOOST Interactive Motor",
89+
46: "Technic Large Motor",
90+
47: "Technic Extra Large Motor",
91+
48: "SPIKE Medium Angular Motor",
92+
49: "SPIKE Large Angular Motor",
93+
61: "SPIKE Color Sensor",
94+
62: "SPIKE Ultrasonic Sensor",
95+
63: "SPIKE Force Sensor",
96+
64: "SPIKE 3x3 Color Light Matrix",
97+
65: "SPIKE Small Angular Motor",
98+
75: "Technic Medium Angular Motor",
99+
76: "Technic Large Angular Motor",
100+
}
101+
102+
def ConnectToDevice(port):
103+
# Returns a device dict()
104+
device = {'type':None, 'id':None, 'name':None, 'object':None}
105+
106+
try: # to get the device, if it is attached.
107+
pupdev = PUPDevice(port)
108+
except OSError as ex:
109+
if ex.args[0] == ENODEV:
110+
# No device found on this port.
111+
return device
112+
else:
113+
raise
114+
# Get the device id
115+
temp_info = pupdev.info()
116+
device['id'] = temp_info["id"]
117+
try: # to look up the name.
118+
device['name'] = device_names[device['id']]
119+
except KeyError:
120+
device['name'] = "Unknown"
121+
#print(port, ":", "Unknown device with ID", xid)
122+
if len(temp_info) > 1:
123+
print(temp_info)
124+
125+
# Initiate object and type
126+
xid = device['id']
127+
if xid in (1,2):
128+
try:
129+
device['object'] = DCMotor(port)
130+
device['type'] = "DCMotor"
131+
except OSError as err:
132+
print("DCMotor could not be initiated: ", err)
133+
device['type'] = "Custom"
134+
pass
135+
elif xid in (46,47,48,49,75,76):
136+
try:
137+
device['object'] = Motor(port, positive_direction=Direction.CLOCKWISE, gears=None)
138+
device['type'] = "Motor"
139+
except:
140+
print("Motor could not be initiated: ", err)
141+
device['type'] = "Custom"
142+
pass
143+
elif xid is 8:
144+
try:
145+
device['object'] = Light(port)
146+
device['object'].on(brightness=50)
147+
device['type'] = "Light"
148+
except:
149+
print("Light could not be initiated: ", err)
150+
device['type'] = "Custom"
151+
pass
152+
elif xid is None: #ToDo
153+
try:
154+
device['object'] = ColorLightMatrix(port)
155+
device['type'] = "Matrix3x3"
156+
except:
157+
print("Matrix could not be initiated: ", err)
158+
device['type'] = "Custom"
159+
pass
160+
elif xid in (34,35,37,61,62,63):
161+
device['type'] = "Sensor"
162+
if xid is 34:
163+
try:
164+
device['object'] = TiltSensor(port)
165+
device['type'] += "/Tilt"
166+
except:
167+
print("TiltSensor could not be initiated: ", err)
168+
device['type'] += "/Custom"
169+
pass
170+
elif xid is 35:
171+
try:
172+
device['object'] = InfraredSensor(port)
173+
device['type'] += "/IR/Distance"
174+
except:
175+
print("InfraredSensor could not be initiated: ", err)
176+
device['type'] += "/Custom"
177+
pass
178+
elif xid is 37:
179+
try:
180+
device['object'] = ColorDistanceSensor(port)
181+
device['type'] += "/Distance/Color/Light"
182+
except:
183+
print("ColorDistanceSensor could not be initiated: ", err)
184+
device['type'] += "/Custom"
185+
pass
186+
elif xid is 61:
187+
try:
188+
device['object'] = ColorSensor(port)
189+
device['type'] += "/Color/Light"
190+
except:
191+
print("ColorSensor could not be initiated: ", err)
192+
device['type'] += "/Custom"
193+
pass
194+
elif xid is 62:
195+
try:
196+
device['object'] = UltrasonicSensor(port)
197+
device['type'] += "/Distance/Light"
198+
except:
199+
print("UltrasonicSensor could not be initiated: ", err)
200+
device['type'] += "/Custom"
201+
pass
202+
elif xid is 63:
203+
try:
204+
device['object'] = ForceSensor(port)
205+
device['type'] += "/Force/Distance/Press"
206+
except:
207+
print("ForceSensor could not be initiated: ", err)
208+
device['type'] += "/Custom"
209+
pass
210+
else:
211+
pass
212+
213+
return device
214+
215+
# end of ConnectToDevice(port)
216+
# -------------------------------------------------------------------
217+
218+
# Make a list of known ports.
219+
ports = [Port.A, Port.B]
220+
try: # to add more ports, on hubs that support it.
221+
ports.append(Port.C)
222+
ports.append(Port.D)
223+
except AttributeError:
224+
pass
225+
try: # to add more ports, on hubs that support it.
226+
ports.append(Port.E)
227+
ports.append(Port.F)
228+
except AttributeError:
229+
pass
230+
231+
# 3: Remote buttons check and remote init
232+
# -------------------------------------------------------------------
233+
remote = None
234+
ch1_val = 0 # +/-100%, scale if needed
235+
ch2_val = 0 # +100%, scale if needed
236+
237+
def ConnectRemote():
238+
global remote
239+
try:
240+
remote = Remote(name=None,timeout=10000)
241+
print("Remote: " + remote.name())
242+
#remote.name("Remote of <user>")
243+
except OSError as ex:
244+
if ex.errno == ETIMEDOUT:
245+
print("No Remote found.")
246+
247+
def ServiceRemote():
248+
global remote
249+
250+
if remote is None:
251+
return (ch1_val,ch2_val)
252+
try:
253+
pressed = remote.buttons.pressed()
254+
except OSError as ex:
255+
if not ex.errno == ENODEV:
256+
raise
257+
print("Lost remote")
258+
remote = None # empty handle
259+
return (ch1_val,ch2_val)
260+
if len(pressed) is 0:
261+
return (ch1_val,ch2_val)
262+
#print(pressed)
263+
264+
ch1_val_new = ch1_val
265+
ch2_val_new = ch2_val
266+
267+
# Remote channel 1:
268+
if Button.LEFT_PLUS in pressed:
269+
ch1_val_new += 10
270+
if ch1_val_new > 100:
271+
ch1_val_new = 100
272+
if Button.LEFT_MINUS in pressed:
273+
ch1_val_new -= 10
274+
if ch1_val_new < -100:
275+
ch1_val_new = -100
276+
if Button.LEFT in pressed:
277+
ch1_val_new = 0
278+
# Remote channel 2:
279+
if Button.RIGHT_PLUS in pressed:
280+
ch2_val_new += 10
281+
if ch2_val_new > 100:
282+
ch2_val_new = 100
283+
if Button.RIGHT_MINUS in pressed:
284+
ch2_val_new -= 10
285+
if ch2_val_new < 0:
286+
ch2_val_new = 0
287+
if Button.RIGHT in pressed:
288+
ch2_val_new = 0
289+
290+
return (ch1_val_new,ch2_val_new)
291+
292+
293+
# 4: Main / Monitor changes
294+
# -------------------------------------------------------------------
295+
DIAGNOSTICS_PERIOD = 5000 # 5s
296+
sys_tick = StopWatch()
297+
(hub,hub_info) = HubInit()
298+
print(hub_info)
299+
pressed = ()
300+
301+
# Readings
302+
tilt = None
303+
distance = None
304+
color = None
305+
force = None
306+
307+
# Search through all available ports.
308+
devices = []
309+
for port in ports:
310+
dev = ConnectToDevice(port)
311+
if dev['type'] is None:
312+
print(port, ": ---")
313+
else:
314+
print(port,dev['id'],":",dev['name'],":",dev['type'])
315+
devices.append(dev)
316+
317+
ConnectRemote()
318+
319+
while True:
320+
wait(100)
321+
try:
322+
pressed = hub.buttons.pressed()
323+
except AttributeError:
324+
pass
325+
try:
326+
pressed = hub.button.pressed()
327+
except AttributeError:
328+
pass
329+
330+
if sys_tick.time() % DIAGNOSTICS_PERIOD:
331+
print("Hub voltage: ", hub.battery.voltage(), "mV")
332+
print("Hub current: ", hub.battery.current(), "mA")
333+
for device in devices:
334+
if "Tilt" in device['type']:
335+
tilt = device['object'].tilt()
336+
if "Distance" in device['type']:
337+
distance = device['object'].distance()
338+
if "Color" in device['type']:
339+
color = device['object'].color()
340+
if "Force" in device['type']:
341+
force = device['object'].force()
342+
print("T:",tilt,"D:",distance,"C:",color,"F:",force)
343+
344+
# do not set values blindly to not interfere with other code:
345+
(ch1_val_new,ch2_val_new) = ServiceRemote()
346+
if ch1_val_new is not ch1_val:
347+
ch1_val = ch1_val_new
348+
print("Channel 1 changed:",ch1_val)
349+
if ch2_val_new is not ch2_val:
350+
ch2_val = ch2_val_new
351+
print("Channel 2 changed:",ch2_val)

0 commit comments

Comments
 (0)