Exemple de gestion d'obstacle avec 5 capteurs

Un exemple de programme python pour la gestion d’obstacles avec les5 capteurs de distances (ToF).

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

from mrpiZ_lib import *
import time, sys
import random

#main program
print "Exemple : Obstacle avoidance"

limit = 20
speed = 45
speedSlow = 30

# demmarage systeme
time.sleep(2)

compteur = 0

try:
while 1:
p1 = proxSensor(1)
p2 = proxSensor(2)
p3 = proxSensor(3)
p4 = proxSensor(4)
p5 = proxSensor(5)

print "lecture prox\n %d", compteur
if (compteur < 1000):
compteur = compteur +1
else:
compteur = 0

if((p1 < limit)or(p2 < limit)) and (p4 > limit):
turnRight(speed)
time.sleep(0.5)
r = random.randint(0,2)
if (r == 0):
back(speedSlow)
elif ((p4 < limit)or(p5 < limit)) and (p2 > limit):
turnLeft(speed)
time.sleep(0.5)
r = random.randint(0,2)
if (r == 1):
back(speedSlow)
elif (p3 < limit):
r = random.randint(0,3)
if (r == 0):
back(speedSlow)
time.sleep(0.6)
elif (r == 1):
back(speedSlow)
time.sleep(0.9)
turnRight(35)
time.sleep(0.5)
elif (r == 2):
back(speedSlow)
time.sleep(0.7)
turnLeft(35)
time.sleep(0.5)
elif (r == 3):
back(speedSlow)
elif (p2 < limit) and (p4 < limit):
turnLeft(35)
time.sleep(1)
else:
forward(speed)
except KeyboardInterrupt:
print "Fin programme"
stop()
resetUc()
sys.exit(0)