Python et le NXT v1
Il y a environ 3 ans, j’avais emprunté un robot NXT de Mindstorms d’un ami à moi histoire de faire joujou avec, car j’étais impressionné par la multitude de combinaisons qu’offrait ce magnifique robot Lego. J’ai par la suite écrit une petite classe Python qui permettait de contrôler ce petit bonhomme. Pour cela, j’avais un peu souffert à l’époque pour mettre en place mon petit environnement de développement, car le robot n’était pas encore aussi populaire que maintenant, mais j’avais quand même réussi à me procurer une petite librairie écrite par un autre développeur dont je ne me rappelle plus du nom (honte sur moi!), mais qui m’avait beaucoup aider à avancer dans mes devs.
Je précise qu’à la base, j’ai écrit cette classe Python pour mon usage interne uniquement! Cependant, je fournis le code ci-dessous, pour les curieux:
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
from nxt.motor import *
from nxt.sensor import *
from time import sleep
import sys, nxt.locator, nxt.brick
class NXT_lego:
"""
Une class pour le robot NXT, et qui possede les methodes suivantes:
- __run_motor(self, l_fwd, r_fwd, l_rot, r_rot): active les moteurs.
- forward(self, rotations): Avance le NXT de rotations de roues.
- backward(self, rotations): Recule le NXT de rotations de roues.
- left(self, degrees): Tourner le NXT vers la gauche de degres.
- right(self, degrees): Tourner le NXT vers la droite de degres.
- close(self): Fermer la connexion Bluetooth / USB du NXT.
- get_ultrasound(self): Retourne la valeur actuelle du capteur ultrason du NXT.
- get_touch(self): Retourne la valeur actuelle du capteur de toucher du NXT.
- get_sound(self): Retourne la valeur actuelle du capteur de son du NXT.
- get_light(self): Retourne la valeur actuelle du capteur de lumiere du NXT.
- play_tone(self, freq, ms=500): Joue une note avec une frequence pendant millisecondes.
"""
NAME = "Lego PyNXT Mindstorm"
VERSION = "9.24"
AUTHOR = "Chegham wassim [contact@cheghamwassim.com]"
DESC = "Let us control the Lego NXT robot with Python!"
WHEEL_POWER =80 # Percentage power to apply to servo motors
FREQ_C = 523
FREQ_C_SHARP = 554
FREQ_D_FLAT = FREQ_C_SHARP
FREQ_D = 587
FREQ_D_SHARP = 622
FREQ_E_FLAT = FREQ_D_SHARP
FREQ_E = 659
FREQ_F = 699
FREQ_F_SHARP = 740
FREQ_G_FLAT = FREQ_F_SHARP
FREQ_G = 784
FREQ_G_SHARP = 831
FREQ_A_FLAT = FREQ_G_SHARP
FREQ_A = 880
FREQ_A_SHARP = 932
FREQ_B_FLAT = FREQ_A_SHARP
FREQ_B = 988
FREQ_C2 = 1047
def __init__(self):
pass
def connect(self, sock=None):
if sock:
self.debug("A socket was given, let's use it! Please wait.... ")
self.sock = sock
return
else :
try :
#self.__debug("No socket given. Trying to find the default socket, please wait... ")
self.sock = nxt.locator.find_one_brick()
if self.sock:
#self.__debug("Found one NXT brick. Trying to establish connection, please wait... ")
pass
self.brick = self.sock.connect()
if self.brick:
self.__debug("Connection established successfully with the NXT module!!")
# Setup sensors
self.touch = TouchSensor(self.brick, PORT_1)
self.sound = SoundSensor(self.brick, PORT_2)
self.light = LightSensor(self.brick, PORT_3)
self.ultrasound = UltrasonicSensor(self.brick, PORT_4)
# Setup motors
self.wheel_m = Motor(self.brick, PORT_A)
self.wheel_l = Motor(self.brick, PORT_B)
self.wheel_r = Motor(self.brick, PORT_C)
except nxt.locator.BrickNotFoundError:
self.__debug("No NXT bricks found...")
#sys.exit()
def __debug(self, message):
print ">> "+message
def __debugError(self, txt):
self.__debug("Error: "+txt)
self.__debug("Help: please check the following:")
self.__debug("\t- The NXT module is plugged to your PC and turned ON.")
self.__debug("\t- You pressed the Connect button on the GUI (if you are using the GUI).")
def __run_motor(self, l_fwd, r_fwd, l_rot, r_rot):
# Set forward or backward motion for each wheel
try:
if l_fwd:
l_power = self.WHEEL_POWER
else:
l_power = self.WHEEL_POWER * -1
if r_fwd:
r_power = self.WHEEL_POWER
else:
r_power = self.WHEEL_POWER * -1
# Left wheel
self.wheel_l.power = l_power
self.wheel_l.mode = MODE_MOTOR_ON
self.wheel_l.run_state = RUN_STATE_RUNNING
self.wheel_l.tacho_limit = l_rot
self.wheel_l.set_output_state()
# Right wheel
self.wheel_r.power = r_power
self.wheel_r.mode = MODE_MOTOR_ON
self.wheel_r.run_state = RUN_STATE_RUNNING
self.wheel_r.tacho_limit = r_rot
self.wheel_r.set_output_state()
except AttributeError:
self.__debugError("could not run the Motor-B&C Servos!")
def __run_third_wheel(self, fwd, rot):
# Set forward or backward motion for the third wheel
try:
if fwd:
power = self.WHEEL_POWER
else:
power = self.WHEEL_POWER * -1
if fwd:
power = self.WHEEL_POWER
else:
power = self.WHEEL_POWER * -1
self.wheel_m.power = power
self.wheel_m.mode = MODE_MOTOR_ON
self.wheel_m.run_state = RUN_STATE_RUNNING
self.wheel_m.tacho_limit = rot
self.wheel_m.set_output_state()
except AttributeError:
self.__debugError("could not run the Motor-A Servo!")
def third_wheel_clockwise(self, rotations=10):
"""Tourne le 3eme moteur du NXT de rotations dans le sens des
aiguielles d'une montre
"""
self.__run_third_wheel(True, rotations)
def third_wheel_anticlockwise(self, rotations):
"""Tourne le 3eme moteur du NXT de rotations dans le sens
contraire des aiguielles d'une montre
"""
self.__run_third_wheel(False, rotations)
def catch(self, rotations=10, n=2):
"""Attrape un objet en ouvrant les pinces puis en les refermant fois
"""
i = 1
while i< =n:
self.third_wheel_clockwise(rotations)
sleep(0.3)
self.third_wheel_anticlockwise(rotations)
if i != n:
sleep(0.5)
i = i+1
return self.__debug("Trying to catch an object "+str(n)+" times by "+str(rotations)+" rotations of the servo.")
def forward(self, rotations=5):
"""Avance le NXT de rotations de roues.
"""
self.__run_motor(True, True, rotations, rotations)
return self.__debug("Moving forward the NXT by "+str(rotations)+" rotations.")
def backward(self, rotations=5):
"""Recule le NXT de rotations de roues.
"""
self.__run_motor(False, False, rotations, rotations)
return self.__debug("Moving backward the NXT by "+str(rotations)+" rotations.")
def left(self, degrees=5):
"""Tourner le NXT vers la gauche de degres.
"""
self.__run_motor(True, True, 1, degrees)
return self.__debug("Turning left the NXT by "+str(degrees)+" degree.")
def right(self, degrees=5):
"""Tourner le NXT vers la droite de degres.
"""
self.__run_motor(True, True, degrees, 1)
return self.__debug("Turning right the NXT by "+str(degrees)+" degree.")
def close(self):
"""Fermer la connexion Bluetooth / USB du NXT.
"""
try:
self.sock.close()
self.__debug("Connection closed!")
except AttributeError:
self.__debugError("could not close the connection with the NXT module!")
def get_ultrasound(self):
"""Retourne la valeur actuelle du capteur ultrason du NXT.
"""
try:
val = self.ultrasound.get_sample()
return self.__debug("Ultrasound:\t"+str(val))
except AttributeError:
self.__debugError("could not get the Ultrasound value!")
def get_touch(self):
"""Return the current reading from the touch sensor.
"""
try:
if self.touch.get_sample():
val = True
return self.__debug("Touch:\t"+str(val))
val = False
return self.__debug("Touch:\t"+str(val))
except AttributeError:
self.__debugError("could not get the Touch status!")
def get_sound(self):
"""Retourne la valeur actuelle du capteur de son du NXT.
"""
try:
val = self.sound.get_sample()
return self.__debug("Sound:\t"+str(val))
except AttributeError:
self.__debugError("could not get the Sound value!")
def get_light(self):
"""Retourne la valeur actuelle du capteur de lumiere du NXT.
"""
try:
val = self.light.get_sample()
return self.__debug("Lights:\t"+str(val))
except AttributeError:
self.__debugError("could not get the Light value!")
def play_tone(self, freq=FREQ_C, ms=500):
"""Joue une note avec une frequence pendant millisecondes.
"""
self.brick.play_tone_and_wait(freq, ms)
sleep(0.5)
def test_ultrasound(self):
a = self.get_ultrasound()
while a > 8:
print a
if a < 100:
self.catch(20) # il ouvre la pince
sleep(0.5) # ce sleep est important, ca laisse le temps au NXT de fermer
# sa pince, sinon, il commence a deconner
a = self.get_ultrasound()
self.__debug("STOP at "+str(a))
def test_all(self):
self.__debug("Test All sensors: ")
self.__debug("Sound:\t\t" + str(self.get_sound()))
self.__debug("Light:\t\t" + str(self.get_light()))
self.__debug("Touch:\t\t" + str(self.get_touch()))
self.__debug("Ultrasound:\t\t" + str(self.get_ultrasound()))
self.forward(90)
sleep(0.3)
self.right(90)
sleep(0.3)
self.catch()
sleep(0.3)
self.left(90)
sleep(0.3)
self.backward(90)
def __parse_info(self, sock):
prot_version, fw_version = sock.get_firmware_version()
print 'Protocol version %s.%s' % prot_version
print 'Firmware version %s.%s' % fw_version
name, host, signal_strength, user_flash = sock.get_device_info()
print 'NXT brick name: %s' % name
print 'Host address: %s' % host
print 'Bluetooth signal strength: %s' % signal_strength
print 'Free user flash: %s' % user_flash
millivolts = sock.get_battery_level()
print 'Battery level %s mV' % millivolts
print '\nModules:'
# FIXME: change to "with" syntax for python 2.5
# with nxt.brick.ModuleFinder(b, '*.*') as m:
# for values in m:
# print '\t'.join('%s' % v for v in values)
m = nxt.brick.ModuleFinder(sock, '*.*')
try:
for values in m:
print '\t'.join('%s' % v for v in values)
finally:
m.__exit__(None, None, None)
def get_info(self):
try:
socks = nxt.locator.find_bricks()
for sock in socks:
self.__parse_info(sock.connect())
sock.close()
except AttributeError:
self.__debugError("The NXT module was not found!")
if __name__ == '__main__':
lego_nxt = NXT_lego()
lego_nxt.test_ultrasound()
lego_nxt.catch()