-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcarmichael.py
executable file
·507 lines (459 loc) · 12.5 KB
/
carmichael.py
1
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
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
#!/usr/bin/python
import argparse
import sys
import os
import time
import RPi.GPIO as gpio
import random
from bluetooth import *
def init():
gpio.setmode(gpio.BOARD)
gpio.setwarnings(False)
gpio.setup(15, gpio.OUT, initial = 0) # H bridge IN1
gpio.setup(13, gpio.OUT, initial = 0) # H bridge IN2
gpio.setup(11, gpio.OUT, initial = 0) # H bridge IN3
gpio.setup(12, gpio.OUT, initial = 0) # H bridge IN4
gpio.setup(3, gpio.OUT, initial = 0) # Ultra sonic sensor out
gpio.setup(7, gpio.IN) # Ultra sonic sensor in
gpio.setup(16, gpio.OUT) # Piezo buzzer
# Motors init
global duty_cycle
duty_cycle = 40
freq = 300
global p11
global p12
global p13
global p15
global p16 # Piezo buzzer
p11 = gpio.PWM(11, freq)
p12 = gpio.PWM(12, freq)
p13 = gpio.PWM(13, freq)
p15 = gpio.PWM(15, freq)
p16 = gpio.PWM(16, 100)
# Notes
global NOTE_B0
global NOTE_C1
global NOTE_CS1
global NOTE_D1
global NOTE_DS1
global NOTE_E1
global NOTE_F1
global NOTE_FS1
global NOTE_G1
global NOTE_GS1
global NOTE_A1
global NOTE_AS1
global NOTE_B1
global NOTE_C2
global NOTE_CS2
global NOTE_D2
global NOTE_DS2
global NOTE_E2
global NOTE_F2
global NOTE_FS2
global NOTE_G2
global NOTE_GS2
global NOTE_A2
global NOTE_AS2
global NOTE_B2
global NOTE_C3
global NOTE_CS3
global NOTE_D3
global NOTE_DS3
global NOTE_E3
global NOTE_F3
global NOTE_FS3
global NOTE_G3
global NOTE_GS3
global NOTE_A3
global NOTE_AS3
global NOTE_B3
global NOTE_C4
global NOTE_CS4
global NOTE_D4
global NOTE_DS4
global NOTE_E4
global NOTE_F4
global NOTE_FS4
global NOTE_G4
global NOTE_GS4
global NOTE_A4
global NOTE_AS4
global NOTE_B4
global NOTE_C5
global NOTE_CS5
global NOTE_D5
global NOTE_DS5
global NOTE_E5
global NOTE_F5
global NOTE_FS5
global NOTE_G5
global NOTE_GS5
global NOTE_A5
global NOTE_AS5
global NOTE_B5
global NOTE_C6
global NOTE_CS6
global NOTE_D6
global NOTE_DS6
global NOTE_E6
global NOTE_F6
global NOTE_FS6
global NOTE_G6
global NOTE_GS6
global NOTE_A6
global NOTE_AS6
global NOTE_B6
global NOTE_C7
global NOTE_CS7
global NOTE_D7
global NOTE_DS7
global NOTE_E7
global NOTE_F7
global NOTE_FS7
global NOTE_G7
global NOTE_GS7
global NOTE_A7
global NOTE_AS7
global NOTE_B7
global NOTE_C8
global NOTE_CS8
global NOTE_D8
global NOTE_DS8
NOTE_B0 = 31
NOTE_C1 = 33
NOTE_CS1 = 35
NOTE_D1 = 37
NOTE_DS1 = 39
NOTE_E1 = 41
NOTE_F1 = 44
NOTE_FS1 = 46
NOTE_G1 = 49
NOTE_GS1 = 52
NOTE_A1 = 55
NOTE_AS1 = 58
NOTE_B1 = 62
NOTE_C2 = 65
NOTE_CS2 = 69
NOTE_D2 = 73
NOTE_DS2 = 78
NOTE_E2 = 82
NOTE_F2 = 87
NOTE_FS2 = 93
NOTE_G2 = 98
NOTE_GS2 = 104
NOTE_A2 = 110
NOTE_AS2 = 117
NOTE_B2 = 123
NOTE_C3 = 131
NOTE_CS3 = 139
NOTE_D3 = 147
NOTE_DS3 = 156
NOTE_E3 = 165
NOTE_F3 = 175
NOTE_FS3 = 185
NOTE_G3 = 196
NOTE_GS3 = 208
NOTE_A3 = 220
NOTE_AS3 = 233
NOTE_B3 = 247
NOTE_C4 = 262
NOTE_CS4 = 277
NOTE_D4 = 294
NOTE_DS4 = 311
NOTE_E4 = 330
NOTE_F4 = 349
NOTE_FS4 = 370
NOTE_G4 = 392
NOTE_GS4 = 415
NOTE_A4 = 440
NOTE_AS4 = 466
NOTE_B4 = 494
NOTE_C5 = 523
NOTE_CS5 = 554
NOTE_D5 = 587
NOTE_DS5 = 622
NOTE_E5 = 659
NOTE_F5 = 698
NOTE_FS5 = 740
NOTE_G5 = 784
NOTE_GS5 = 831
NOTE_A5 = 880
NOTE_AS5 = 932
NOTE_B5 = 988
NOTE_C6 = 1047
NOTE_CS6 = 1109
NOTE_D6 = 1175
NOTE_DS6 = 1245
NOTE_E6 = 1319
NOTE_F6 = 1397
NOTE_FS6 = 1480
NOTE_G6 = 1568
NOTE_GS6 = 1661
NOTE_A6 = 1760
NOTE_AS6 = 1865
NOTE_B6 = 1976
NOTE_C7 = 2093
NOTE_CS7 = 2217
NOTE_D7 = 2349
NOTE_DS7 = 2489
NOTE_E7 = 2637
NOTE_F7 = 2794
NOTE_FS7 = 2960
NOTE_G7 = 3136
NOTE_GS7 = 3322
NOTE_A7 = 3520
NOTE_AS7 = 3729
NOTE_B7 = 3951
NOTE_C8 = 4186
NOTE_CS8 = 4435
NOTE_D8 = 4699
NOTE_DS8 = 4978
global buzzer_speed
def autonomy():
delay = 0.1
degr_90 = 1.0/(duty_cycle/30.0)
print("degre_90: " + str(degr_90))
time.sleep(delay)
dist = distance('cm')
print dist
if dist < 25:
stop()
#print("Setting buzzer set to HIGH")
#gpio.output(16, gpio.HIGH)
#print("Sleeping for 0.1 sec")
#time.sleep(0.1)
#print("Setting buzzer to LOW")
#gpio.output(16, gpio.LOW)
gpio.output(16, True)
buzzer_speed = 0.2
p16.start(10) # 10% duty cycle sounds 'ok'
p16.ChangeFrequency(200)
time.sleep(buzzer_speed)
p16.ChangeFrequency(100)
time.sleep(buzzer_speed)
p16.stop()
rand = random.randrange(0,2);
if rand == 0:
pivotLeft(degr_90)
elif rand == 1:
pivotRight(degr_90)
elif rand == 2:
back()
print("Pivoting left")
else:
forward()
print("Going forward")
def distance(measure='cm'):
sig = 0
nosig = 0
gpio.output(3, True)
gpio.output(3, False)
while gpio.input(7) == 0:
nosig = time.time()
while gpio.input(7) == 1:
sig = time.time()
tl = sig - nosig
if measure == 'cm':
distance = tl / 0.000058
elif measure == 'in':
distance = tl / 0.000148
else:
print("Improper choice of measurement: in or cm")
distance = None
return distance
# Pivot Left function
def pivotRight(runtime = 0.05):
p12.start(duty_cycle)
p11.stop()
p13.stop()
p15.start(duty_cycle)
time.sleep(runtime)
return
# Pivot Right function
def pivotLeft(runtime = 0.05):
p12.stop()
p11.start(duty_cycle)
p13.start(duty_cycle)
p15.stop()
time.sleep(runtime)
return
# Forward Left function
def backLeft(runtime = 0.05):
p12.start(duty_cycle)
p11.stop()
p13.stop()
p15.stop()
time.sleep(runtime)
return
# Forward Right function
def backRight(runtime = 0.05):
p13.start(duty_cycle)
p11.stop()
p12.stop()
p15.stop()
time.sleep(runtime)
return
# Back Left function
def forwardLeft(runtime = 0.05):
p11.start(duty_cycle)
p12.stop()
p13.stop()
p15.stop()
time.sleep(runtime)
return
# Back Right function
def forwardRight(runtime = 0.05):
p15.start(duty_cycle)
p11.stop()
p12.stop()
p13.stop()
time.sleep(runtime)
return
# Forward function
def back(runtime = 0.05):
p12.start(duty_cycle)
p13.start(duty_cycle)
p11.stop()
p15.stop()
time.sleep(runtime)
return
# Back function
def forward(runtime = 0.05):
p11.start(duty_cycle)
p15.start(duty_cycle)
p12.stop()
p13.stop()
time.sleep(runtime)
return
# Stop function
def stop():
p11.stop()
p12.stop()
p13.stop()
p15.stop()
return
# Main loop
def main():
global duty_cycle
###print "Start"
# We need to wait until Bluetooth init is done
time.sleep(1)
# Make device visible
os.system("hciconfig hci0 piscan")
# Create a new server socket using RFCOMM protocol
server_sock = BluetoothSocket(RFCOMM)
# Bind to any port
server_sock.bind(("", PORT_ANY))
# Start listening
server_sock.listen(1)
# Get the port the server socket is listening
port = server_sock.getsockname()[1]
# The service UUID to advertise
uuid = "7be1fcb3-5776-42fb-91fd-2ee7b5bbb86d"
# Start advertising the service
advertise_service(server_sock, "RaspiBtSrv",
service_id=uuid,
service_classes=[uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE])
# Main Bluetooth server loop
while True:
init()
# Startup completed sound
gpio.output(16, True)
buzzer_speed = 0.2
p16.start(10)
p16.ChangeFrequency(NOTE_E6)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_G6)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_E7)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_C7)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_D7)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_G7)
time.sleep(buzzer_speed)
p16.stop()
print "Waiting for connection on RFCOMM channel %d" % port
try:
client_sock = None
# This will block until we get a new connection
print("Please connect bluetooth controller")
client_sock, client_info = server_sock.accept()
print "Accepted connection from ", client_info
# Blutetooth connected sound
gpio.output(16, True)
buzzer_speed = 0.2
p16.start(10)
p16.ChangeFrequency(NOTE_A5)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_B5)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_C5)
time.sleep(buzzer_speed)
p16.ChangeFrequency(NOTE_B5)
time.sleep(buzzer_speed)
p16.stop()
auto = False # Begin with manual control. Sets to True when car should be autonomous.
# Read the data sent by the client
while True:
###print "Waiting for command"
data = client_sock.recv(1024)
print "Received: %s" % repr(data)
if auto == False:
if ("F" in data):
print "Moving forward"
forward(0.05)
elif ("B" in data):
print "Moving backward"
back(0.05)
elif ("L" in data):
print "Pivot left"
pivotLeft(0.05)
elif ("R" in data):
print "Pivot right"
pivotRight(0.05)
elif ("G" in data):
print "Forward left"
forwardLeft(0.05)
elif ("I" in data):
print "Forward right"
forwardRight(0.05)
elif ("H" in data):
print "back left"
backLeft(0.05)
elif ("J" in data):
print "Back right"
backRight(0.05)
elif ("S" in data):
stop()
elif (data in str(range(9))):
print("Changing speed to: " + data)
duty_cycle = 10 + int(data) * 10
print("Duty cycle is: " + str(duty_cycle))
elif ("V" in data):
auto = not auto
elif (data == "q\r\n"):
stop()
print ("Quit")
break
elif auto == True:
if ("v" in data):
auto = not auto
else:
autonomy()
except IOError:
stop()
pass
except KeyboardInterrupt:
if client_sock is not None:
client_sock.close()
server_sock.close()
stop()
print "Server going down"
break
main()
gpio.cleanup()
print("Cleanup")