11from pybricks .geometry import Axis
22from pybricks .iodevices import PUPDevice
3- from pybricks .pupdevices import *
4- from pybricks .parameters import Port , Button , Color # if needed: Side
3+ from pybricks .pupdevices import DCMotor , Motor , Light , ColorLightMatrix , Remote , TiltSensor , InfraredSensor , ColorDistanceSensor , ColorSensor , UltrasonicSensor , ForceSensor
4+ from pybricks .parameters import Port , Button , Color , Direction # if needed: Side
55from pybricks .tools import wait , StopWatch
66from uerrno import ENODEV , ETIMEDOUT
7+ from micropython import const
78
89# 1: Determine the type of hub
910# -------------------------------------------------------------------
@@ -73,12 +74,11 @@ def HubInit(mounted_top=Axis.Z, mounted_front=Axis.X):
7374
7475 return (hub , hub_info )
7576
77+
7678# 2: Diagnose connected devices
7779# -------------------------------------------------------------------
7880# Dictionary of device identifiers along with their name.
7981# Also check https://github.com/pybricks/technical-info/blob/master/assigned-numbers.md#io-device-type-ids
80-
81-
8282device_names = {
8383 # pybricks.pupdevices.DCMotor
8484 1 : "Wedo 2.0 Medium Motor" ,
@@ -111,6 +111,21 @@ def HubInit(mounted_top=Axis.Z, mounted_front=Axis.X):
111111}
112112
113113
114+ def GetPorts ():
115+ ports = [Port .A , Port .B ]
116+ try : # to add more ports, on hubs that support it.
117+ ports .append (Port .C )
118+ ports .append (Port .D )
119+ except AttributeError :
120+ pass
121+ try : # to add more ports, on hubs that support it.
122+ ports .append (Port .E )
123+ ports .append (Port .F )
124+ except AttributeError :
125+ pass
126+ return ports
127+
128+
114129def ConnectToDevice (port ):
115130 # Returns a device dict()
116131 device = {'type' : None , 'id' : None , 'name' : None , 'object' : None }
@@ -121,123 +136,96 @@ def ConnectToDevice(port):
121136 if ex .args [0 ] == ENODEV :
122137 # No device found on this port.
123138 return device
124- else :
125- raise
139+ raise
140+
126141 # Get the device id
127142 temp_info = pupdev .info ()
128143 device ['id' ] = temp_info ["id" ]
129144 try : # to look up the name.
130145 device ['name' ] = device_names [device ['id' ]]
131146 except KeyError :
132147 device ['name' ] = "Unknown"
133- # print(port, ":", "Unknown device with ID", xid)
134148 if len (temp_info ) > 1 :
135149 print (temp_info )
136150
137151 # Initiate object and type
138152 xid = device ['id' ]
139- if xid in (1 , 2 ):
140- try :
153+ device ['type' ] = "" # Make it work with += "/Custom" in except
154+ try :
155+ if xid in (1 , 2 ):
156+ dev_class = "DCMotor"
141157 device ['object' ] = DCMotor (port )
142158 device ['type' ] = "DCMotor"
143- except OSError as err :
144- print ("DCMotor could not be initiated: " , err )
145- device ['type' ] = "Custom"
146- pass
147- elif xid in (38 , 46 , 47 , 48 , 49 , 65 , 75 , 76 ):
148- try :
159+ elif xid in (38 , 46 , 47 , 48 , 49 , 65 , 75 , 76 ):
160+ dev_class = "Motor"
149161 device ['object' ] = Motor (port , positive_direction = Direction .CLOCKWISE , gears = None )
150162 device ['type' ] = "Motor"
151- except OSError as err :
152- print ("Motor could not be initiated: " , err )
153- device ['type' ] = "Custom"
154- pass
155- elif xid == 8 :
156- try :
163+ elif xid == 8 :
164+ dev_class = "Light"
157165 device ['object' ] = Light (port )
158166 device ['object' ].on (brightness = 50 )
159167 device ['type' ] = "Light"
160- except OSError as err :
161- print ("Light could not be initiated: " , err )
162- device ['type' ] = "Custom"
163- pass
164- elif xid == 64 :
165- try :
168+ elif xid == 64 :
169+ dev_class = "ColorLightMatrix"
166170 device ['object' ] = ColorLightMatrix (port )
167171 device ['object' ].on ([Color .RED , Color .GREEN , Color .BLUE ])
168172 device ['type' ] = "Matrix3x3"
169- except OSError as err :
170- print ("Matrix could not be initiated: " , err )
171- device ['type' ] = "Custom"
172- pass
173- elif xid in (34 , 35 , 37 , 61 , 62 , 63 ):
174- device ['type' ] = "Sensor"
175- sensor_class = None
176- try :
173+ elif xid in (34 , 35 , 37 , 61 , 62 , 63 ):
174+ device ['type' ] = "Sensor"
177175 if xid == 34 :
178- sensor_class = "TiltSensor"
176+ dev_class = "TiltSensor"
179177 device ['object' ] = TiltSensor (port )
180178 device ['type' ] += "/Tilt"
181179 elif xid == 35 :
182- sensor_class = "InfraredSensor"
180+ dev_class = "InfraredSensor"
183181 device ['object' ] = InfraredSensor (port )
184182 device ['type' ] += "/IR/Distance"
185183 elif xid == 37 :
186- sensor_class = "ColorDistanceSensor"
184+ dev_class = "ColorDistanceSensor"
187185 device ['object' ] = ColorDistanceSensor (port )
188186 device ['type' ] += "/Distance/Color/Light"
189187 elif xid == 61 :
190- sensor_class = "ColorSensor"
188+ dev_class = "ColorSensor"
191189 device ['object' ] = ColorSensor (port )
192190 device ['type' ] += "/Color/Light"
193191 elif xid == 62 :
194- sensor_class = "UltrasonicSensor"
192+ dev_class = "UltrasonicSensor"
195193 device ['object' ] = UltrasonicSensor (port )
196194 device ['type' ] += "/Distance/Light"
197195 elif xid == 63 :
198- sensor_class = "ForceSensor"
196+ dev_class = "ForceSensor"
199197 device ['object' ] = ForceSensor (port )
200198 device ['type' ] += "/Force/Distance/Press"
201- except OSError as err :
202- print ("class" , sensor_class , "could not be initiated: " , err )
203- device ['type' ] += "/Custom"
204- pass
205- else :
206- print ("Not able to translate id:" , xid , "to a class!" )
199+ else :
200+ print ("Not able to translate id:" , xid , "to a class!" )
201+ except OSError as err :
202+ print ("class" , dev_class , "could not be initiated: " , err )
203+ device ['type' ] += "/Custom"
207204 pass
205+
208206 return device
209207# end of ConnectToDevice(port)
210208# -------------------------------------------------------------------
211209
212- # Make a list of known ports.
213- ports = [Port .A , Port .B ]
214- try : # to add more ports, on hubs that support it.
215- ports .append (Port .C )
216- ports .append (Port .D )
217- except AttributeError :
218- pass
219- try : # to add more ports, on hubs that support it.
220- ports .append (Port .E )
221- ports .append (Port .F )
222- except AttributeError :
223- pass
224210
225211# 3: Remote buttons check and remote init
226212# -------------------------------------------------------------------
227213remote = None
228214ch1_val = 0 # +/-100%, scale if needed
229215ch2_val = 0 # +100%, scale if needed
230216
217+
231218def ConnectRemote ():
232219 global remote
233220 try :
234- remote = Remote (name = None ,timeout = 10000 )
221+ remote = Remote (name = None , timeout = 10000 )
235222 print ("Remote: " + remote .name ())
236223 # remote.name("Remote of <user>")
237224 except OSError as ex :
238225 if ex .errno == ETIMEDOUT :
239226 print ("No Remote found." )
240227
228+
241229def ServiceRemote ():
242230 global remote
243231
@@ -251,7 +239,7 @@ def ServiceRemote():
251239 print ("Lost remote" )
252240 remote = None # empty handle
253241 return (ch1_val , ch2_val )
254- if len (pressed ) is 0 :
242+ if len (pressed ) == 0 :
255243 return (ch1_val , ch2_val )
256244 # print(pressed)
257245
@@ -284,10 +272,11 @@ def ServiceRemote():
284272 return (ch1_val_new , ch2_val_new )
285273
286274
287- # 4: Main / Monitor changes
275+ # 4: Main loop: Monitor changes
288276# -------------------------------------------------------------------
289- DIAGNOSTICS_PERIOD = 5000 # 5s
277+ DIAGNOSTICS_PERIOD = const ( 2000 ) # 5s
290278sys_tick = StopWatch ()
279+ last_diag = sys_tick .time ()
291280(hub , hub_info ) = HubInit ()
292281print (hub_info )
293282pressed = ()
@@ -297,8 +286,10 @@ def ServiceRemote():
297286distance = None
298287color = None
299288force = None
289+ imu_tilt = None
300290
301291# Search through all available ports.
292+ ports = GetPorts ()
302293devices = []
303294for port in ports :
304295 dev = ConnectToDevice (port )
@@ -320,12 +311,16 @@ def ServiceRemote():
320311 pressed = hub .button .pressed ()
321312 except AttributeError :
322313 pass
323-
324- if sys_tick .time () % DIAGNOSTICS_PERIOD :
314+ if len (pressed ) != 0 :
315+ print ("Hub button(s) pressed:" , pressed )
316+ if (sys_tick .time () - last_diag ) > DIAGNOSTICS_PERIOD :
317+ last_diag = sys_tick .time ()
325318 print ("Hub voltage: " , hub .battery .voltage (), "mV" )
326319 print ("Hub current: " , hub .battery .current (), "mA" )
320+ if hub_info ['has_imu' ] is True :
321+ imu_tilt = hub .imu .tilt ()
327322 for device in devices :
328- if "DCMotor" or " Motor" in device ['type' ]:
323+ if "Motor" in device ['type' ]: # also catches DCMotor
329324 device ['object' ].dc (ch1_val )
330325 if "Tilt" in device ['type' ]:
331326 tilt = device ['object' ].tilt ()
@@ -335,10 +330,10 @@ def ServiceRemote():
335330 color = device ['object' ].color ()
336331 if "Force" in device ['type' ]:
337332 force = device ['object' ].force ()
338- print ("T :" , tilt , "D :" , distance , "C :" , color , "F :" , force )
339-
333+ print ("D :" , distance , "C :" , color , "F :" , force , "T :" , tilt , "IMU.T:" , imu_tilt )
334+
340335 # do not set values blindly to not interfere with other code:
341- (ch1_val_new ,ch2_val_new ) = ServiceRemote ()
336+ (ch1_val_new , ch2_val_new ) = ServiceRemote ()
342337 if ch1_val_new is not ch1_val :
343338 ch1_val = ch1_val_new
344339 print ("Channel 1 changed:" , ch1_val )
0 commit comments