Repository navigation
Expand file tree
/
Copy pathlevel_2.py
More file actions
83 lines (65 loc) · 1.8 KB
/
Copy pathlevel_2.py
File metadata and controls
83 lines (65 loc) · 1.8 KB
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
import time
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
motor1 = [2,3,4,17]
motor2 = [27,22,10,9]
sensorl = 14
sensorm = 15
sensorr = 18
##Seq voor en achter uit
SeqVoorAchter1 = [[1,0,0,0],[1,1,0,0],[0,1,0,0],[0,1,1,0],[0,0,1,0],[0,0,1,1],[0,0,0,1],[1,0,0,1]]
SeqVoorAchter2 = [[0,0,0,1],[0,0,1,1],[0,0,1,0],[0,1,1,0],[0,1,0,0],[1,1,0,0],[1,0,0,0],[1,0,0,1]]
delay = 2/float(1000)
for pin in motor1:
GPIO.setup(pin, GPIO.OUT)
GPIO.output(pin, 0)
for pin in motor2:
GPIO.setup(pin, GPIO.OUT)
GPIO.output(pin, 0)
GPIO.setup(sensorl, GPIO.IN)
GPIO.setup(sensorm, GPIO.IN)
GPIO.setup(sensorr, GPIO.IN)
stepcountVoorAchter1 = len(SeqVoorAchter1)
stepcountVoorAchter2 = len(SeqVoorAchter2)
stepcountvoorAchter = 0
if (stepcountVoorAchter1 == stepcountVoorAchter2):
stepcountVoorAchter = stepcountVoorAchter1
def rij(motor, seq) :
GPIO.output(motor[0], seq[0])
GPIO.output(motor[1], seq[1])
GPIO.output(motor[2], seq[2])
GPIO.output(motor[3], seq[3])
def vooruit(step) :
global motor1
global motor2
rij(motor1, SeqVoorAchter1[step])
rij(motor2, SeqVoorAchter2[step])
def achteruit(step) :
global motor1
global motor2
rij(motor1, SeqVoorAchter2[step])
rij(motor2, SeqVoorAchter1[step])
def rechts(step):
global motor1
global motor2
rij(motor1, SeqVoorAchter1[step])
rij(motor2, SeqVoorAchter1[step])
def links(step):
global motor1
global motor2
rij(motor2, SeqVoorAchter2[step])
rij(motor1, SeqVoorAchter2[step])
step = 0
while True:
if (not GPIO.input(sensorl) and GPIO.input(sensorr)) :
links(step)
elif (GPIO.input(sensorl) and not GPIO.input(sensorr)) :
rechts(step)
else :
vooruit(step)
if(step == stepcountVoorAchter - 1):
step = 0
else :
step = step + 1
time.sleep(delay)