11from pybricks .geometry import Axis
22from pybricks .iodevices import PUPDevice
33from pybricks .pupdevices import *
4- from pybricks .parameters import Port , Button , Color
4+ from pybricks .parameters import Port , Button , Color # if needed: Side
55from pybricks .tools import wait , StopWatch
66from uerrno import ENODEV , ETIMEDOUT
77
88# 1: Determine the type of hub
99# -------------------------------------------------------------------
10- # For Hubs with IMU: how is it mounted?
11- # https://docs.pybricks.com/en/latest/geometry.html#fig-imuexamples
10+ # How is hub with IMU mounted: https://docs.pybricks.com/en/latest/geometry.html#fig-imuexamples
11+
12+
1213def HubInit (mounted_top = Axis .Z , mounted_front = Axis .X ):
1314 # Returns (str hub_object, dict hub_info)
1415 hub = None
@@ -31,7 +32,7 @@ def HubInit(mounted_top=Axis.Z, mounted_front=Axis.X):
3132 hub_info .update (hub_type = "TechnicHub" )
3233 except ImportError :
3334 pass
34- try : # InventorHub is the same as PrimeHub, so treat the same.
35+ try : # InventorHub is the same as PrimeHub, so treat the same.
3536 from pybricks .hubs import PrimeHub
3637 hub = PrimeHub (top_side = mounted_top , front_side = mounted_front )
3738 hub_info .update (hub_type = "PrimeHub" )
@@ -47,24 +48,23 @@ def HubInit(mounted_top=Axis.Z, mounted_front=Axis.X):
4748 if hub is None :
4849 raise Exception ("No valid Hub found" )
4950
50- print ("Hub:\n Name:\t " , hub .system .name (),":" ,hub_info ['hub_type' ])
51+ print ("Hub:\n Name:\t " , hub .system .name (), ":" , hub_info ['hub_type' ])
5152 print (" Reset reason: " , hub .system .reset_reason ())
5253 print (" voltage: " , hub .battery .voltage (), "mV" )
5354 print (" current: " , hub .battery .current (), "mA" )
5455
5556 # Check for IMU and Matrix dislay:
56- try : # to init IMU
57- from pybricks .parameters import Side
57+ try : # to init IMU
5858 hub_info .update (init_position = hub .imu .up ())
5959 print (" facing: " , hub_info ['init_position' ])
6060 hub_info .update (has_imu = True )
6161 except AttributeError :
6262 hub_info .update (has_imu = False )
6363 pass
64- try : # to init Matrix
64+ try : # to init Matrix
6565 from pybricks .parameters import Icon
6666 hub .display .icon (Icon .UP / 2 )
67- hub .display .orientation (up_side )
67+ hub .display .orientation (hub_info [ 'init_position' ] )
6868 print (" with display matrix." )
6969 hub_info .update (has_matrix = True )
7070 except ImportError or AttributeError :
@@ -76,6 +76,9 @@ def HubInit(mounted_top=Axis.Z, mounted_front=Axis.X):
7676# 2: Diagnose connected devices
7777# -------------------------------------------------------------------
7878# 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+
81+
7982device_names = {
8083 # pybricks.pupdevices.DCMotor
8184 1 : "Wedo 2.0 Medium Motor" ,
@@ -109,9 +112,9 @@ def HubInit(mounted_top=Axis.Z, mounted_front=Axis.X):
109112
110113def ConnectToDevice (port ):
111114 # Returns a device dict()
112- device = {'type' :None , 'id' :None , 'name' :None , 'object' :None }
115+ device = {'type' : None , 'id' : None , 'name' : None , 'object' : None }
113116
114- try : # to get the device, if it is attached.
117+ try : # to get the device, if it is attached.
115118 pupdev = PUPDevice (port )
116119 except OSError as ex :
117120 if ex .args [0 ] == ENODEV :
@@ -122,7 +125,7 @@ def ConnectToDevice(port):
122125 # Get the device id
123126 temp_info = pupdev .info ()
124127 device ['id' ] = temp_info ["id" ]
125- try : # to look up the name.
128+ try : # to look up the name.
126129 device ['name' ] = device_names [device ['id' ]]
127130 except KeyError :
128131 device ['name' ] = "Unknown"
@@ -132,19 +135,19 @@ def ConnectToDevice(port):
132135
133136 # Initiate object and type
134137 xid = device ['id' ]
135- if xid in (1 ,2 ):
138+ if xid in (1 , 2 ):
136139 try :
137140 device ['object' ] = DCMotor (port )
138141 device ['type' ] = "DCMotor"
139142 except OSError as err :
140143 print ("DCMotor could not be initiated: " , err )
141144 device ['type' ] = "Custom"
142145 pass
143- elif xid in (46 ,47 ,48 ,49 ,75 ,76 ):
146+ elif xid in (38 , 46 , 47 , 48 , 49 , 65 , 75 , 76 ):
144147 try :
145148 device ['object' ] = Motor (port , positive_direction = Direction .CLOCKWISE , gears = None )
146149 device ['type' ] = "Motor"
147- except :
150+ except OSError as err :
148151 print ("Motor could not be initiated: " , err )
149152 device ['type' ] = "Custom"
150153 pass
@@ -153,65 +156,65 @@ def ConnectToDevice(port):
153156 device ['object' ] = Light (port )
154157 device ['object' ].on (brightness = 50 )
155158 device ['type' ] = "Light"
156- except :
159+ except OSError as err :
157160 print ("Light could not be initiated: " , err )
158161 device ['type' ] = "Custom"
159162 pass
160- elif xid is None : #ToDo
163+ elif xid is None : #ToDo
161164 try :
162165 device ['object' ] = ColorLightMatrix (port )
163166 device ['type' ] = "Matrix3x3"
164- except :
167+ except OSError as err :
165168 print ("Matrix could not be initiated: " , err )
166169 device ['type' ] = "Custom"
167170 pass
168- elif xid in (34 ,35 ,37 ,61 ,62 ,63 ):
171+ elif xid in (34 , 35 , 37 , 61 , 62 , 63 ):
169172 device ['type' ] = "Sensor"
170173 if xid is 34 :
171174 try :
172175 device ['object' ] = TiltSensor (port )
173176 device ['type' ] += "/Tilt"
174- except :
177+ except OSError as err :
175178 print ("TiltSensor could not be initiated: " , err )
176179 device ['type' ] += "/Custom"
177180 pass
178181 elif xid is 35 :
179182 try :
180183 device ['object' ] = InfraredSensor (port )
181184 device ['type' ] += "/IR/Distance"
182- except :
185+ except OSError as err :
183186 print ("InfraredSensor could not be initiated: " , err )
184187 device ['type' ] += "/Custom"
185188 pass
186189 elif xid is 37 :
187190 try :
188191 device ['object' ] = ColorDistanceSensor (port )
189192 device ['type' ] += "/Distance/Color/Light"
190- except :
193+ except OSError as err :
191194 print ("ColorDistanceSensor could not be initiated: " , err )
192195 device ['type' ] += "/Custom"
193196 pass
194197 elif xid is 61 :
195198 try :
196199 device ['object' ] = ColorSensor (port )
197200 device ['type' ] += "/Color/Light"
198- except :
201+ except OSError as err :
199202 print ("ColorSensor could not be initiated: " , err )
200203 device ['type' ] += "/Custom"
201204 pass
202205 elif xid is 62 :
203206 try :
204207 device ['object' ] = UltrasonicSensor (port )
205208 device ['type' ] += "/Distance/Light"
206- except :
209+ except OSError as err :
207210 print ("UltrasonicSensor could not be initiated: " , err )
208211 device ['type' ] += "/Custom"
209212 pass
210213 elif xid is 63 :
211214 try :
212215 device ['object' ] = ForceSensor (port )
213216 device ['type' ] += "/Force/Distance/Press"
214- except :
217+ except OSError as err :
215218 print ("ForceSensor could not be initiated: " , err )
216219 device ['type' ] += "/Custom"
217220 pass
@@ -225,12 +228,12 @@ def ConnectToDevice(port):
225228
226229# Make a list of known ports.
227230ports = [Port .A , Port .B ]
228- try : # to add more ports, on hubs that support it.
231+ try : # to add more ports, on hubs that support it.
229232 ports .append (Port .C )
230233 ports .append (Port .D )
231234except AttributeError :
232235 pass
233- try : # to add more ports, on hubs that support it.
236+ try : # to add more ports, on hubs that support it.
234237 ports .append (Port .E )
235238 ports .append (Port .F )
236239except AttributeError :
@@ -239,8 +242,8 @@ def ConnectToDevice(port):
239242# 3: Remote buttons check and remote init
240243# -------------------------------------------------------------------
241244remote = None
242- ch1_val = 0 # +/-100%, scale if needed
243- ch2_val = 0 # +100%, scale if needed
245+ ch1_val = 0 # +/-100%, scale if needed
246+ ch2_val = 0 # +100%, scale if needed
244247
245248def ConnectRemote ():
246249 global remote
@@ -256,17 +259,17 @@ def ServiceRemote():
256259 global remote
257260
258261 if remote is None :
259- return (ch1_val ,ch2_val )
262+ return (ch1_val , ch2_val )
260263 try :
261264 pressed = remote .buttons .pressed ()
262265 except OSError as ex :
263266 if not ex .errno == ENODEV :
264267 raise
265268 print ("Lost remote" )
266- remote = None # empty handle
267- return (ch1_val ,ch2_val )
269+ remote = None # empty handle
270+ return (ch1_val , ch2_val )
268271 if len (pressed ) is 0 :
269- return (ch1_val ,ch2_val )
272+ return (ch1_val , ch2_val )
270273 #print(pressed)
271274
272275 ch1_val_new = ch1_val
@@ -295,14 +298,14 @@ def ServiceRemote():
295298 if Button .RIGHT in pressed :
296299 ch2_val_new = 0
297300
298- return (ch1_val_new ,ch2_val_new )
301+ return (ch1_val_new , ch2_val_new )
299302
300303
301304# 4: Main / Monitor changes
302305# -------------------------------------------------------------------
303306DIAGNOSTICS_PERIOD = 5000 # 5s
304307sys_tick = StopWatch ()
305- (hub ,hub_info ) = HubInit ()
308+ (hub , hub_info ) = HubInit ()
306309print (hub_info )
307310pressed = ()
308311
@@ -319,7 +322,7 @@ def ServiceRemote():
319322 if dev ['type' ] is None :
320323 print (port , ": ---" )
321324 else :
322- print (port ,dev ['id' ],":" ,dev ['name' ],":" ,dev ['type' ])
325+ print (port ,dev ['id' ], ":" , dev ['name' ], ":" , dev ['type' ])
323326 devices .append (dev )
324327
325328ConnectRemote ()
@@ -339,6 +342,8 @@ def ServiceRemote():
339342 print ("Hub voltage: " , hub .battery .voltage (), "mV" )
340343 print ("Hub current: " , hub .battery .current (), "mA" )
341344 for device in devices :
345+ if "DCMotor" or "Motor" in device ['type' ]:
346+ device ['object' ].dc (ch1_val )
342347 if "Tilt" in device ['type' ]:
343348 tilt = device ['object' ].tilt ()
344349 if "Distance" in device ['type' ]:
@@ -347,13 +352,13 @@ def ServiceRemote():
347352 color = device ['object' ].color ()
348353 if "Force" in device ['type' ]:
349354 force = device ['object' ].force ()
350- print ("T:" ,tilt ,"D:" ,distance ,"C:" ,color ,"F:" ,force )
355+ print ("T:" , tilt , "D:" , distance , "C:" , color , "F:" , force )
351356
352357 # do not set values blindly to not interfere with other code:
353358 (ch1_val_new ,ch2_val_new ) = ServiceRemote ()
354359 if ch1_val_new is not ch1_val :
355360 ch1_val = ch1_val_new
356- print ("Channel 1 changed:" ,ch1_val )
361+ print ("Channel 1 changed:" , ch1_val )
357362 if ch2_val_new is not ch2_val :
358363 ch2_val = ch2_val_new
359- print ("Channel 2 changed:" ,ch2_val )
364+ print ("Channel 2 changed:" , ch2_val )
0 commit comments