-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmoteurs.py
162 lines (133 loc) · 4.48 KB
/
moteurs.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
# a revoir
import time
import RPi.GPIO as GPIO
Motor_A_pin = 4
Motor_B_pin = 17
Motor_A_pin1 = 14
Motor_A_pin2 = 15
Motor_B_pin1 = 27
Motor_B_pin2 = 18
def motorStop():
GPIO.output(Motor_A_pin1, GPIO.LOW)
GPIO.output(Motor_B_pin1, GPIO.LOW)
GPIO.output(Motor_A_pin2, GPIO.LOW)
GPIO.output(Motor_B_pin2, GPIO.LOW)
GPIO.output(Motor_A_pin, GPIO.LOW)
GPIO.output(Motor_B_pin, GPIO.LOW)
def setup():
global pwm_A, pwm_B
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(Motor_A_pin, GPIO.OUT)
GPIO.setup(Motor_B_pin, GPIO.OUT)
GPIO.setup(Motor_A_pin1, GPIO.OUT)
GPIO.setup(Motor_A_pin2, GPIO.OUT)
GPIO.setup(Motor_B_pin1, GPIO.OUT)
GPIO.setup(Motor_B_pin2, GPIO.OUT)
motorStop()
try:
pwm_A = GPIO.PWM(Motor_A_pin, 1000)
pwm_B = GPIO.PWM(Motor_B_pin, 1000)
except:
pass
def motor_A(status, direction, speed):
global pwm_A
if status == 'pause':
GPIO.output(Motor_A_pin, GPIO.LOW)
GPIO.output(Motor_A_pin1, GPIO.LOW)
GPIO.output(Motor_A_pin2, GPIO.LOW)
else:
if direction == 'devant':
GPIO.output(Motor_A_pin1, GPIO.LOW)
GPIO.output(Motor_A_pin2, GPIO.HIGH)
pwm_A.start(100)
pwm_A.ChangeDutyCycle(speed)
elif direction == 'derriere':
GPIO.output(Motor_A_pin1, GPIO.HIGH)
GPIO.output(Motor_A_pin2, GPIO.LOW)
pwm_A.start(0)
pwm_A.ChangeDutyCycle(speed)
def motor_B(status, direction, speed):
global pwm_B
if status == 'pause':
GPIO.output(Motor_B_pin, GPIO.LOW)
GPIO.output(Motor_B_pin1, GPIO.LOW)
GPIO.output(Motor_B_pin2, GPIO.LOW)
else:
if direction == 'devant':
GPIO.output(Motor_B_pin1, GPIO.HIGH)
GPIO.output(Motor_B_pin2, GPIO.LOW)
pwm_B.start(100)
pwm_B.ChangeDutyCycle(speed)
elif direction == 'derriere':
GPIO.output(Motor_B_pin1, GPIO.LOW)
GPIO.output(Motor_B_pin2, GPIO.HIGH)
pwm_B.start(0)
pwm_B.ChangeDutyCycle(speed)
def conduite(direction, cote, vitesse):
if direction == 'devant':
if cote == 'droit':
motor_A('pause', 'devant', vitesse * 0.88) #Les 0.88 sont là pour équilibrer le fait que l'une des chaines est plus molle donc ralentit le robot
motor_B('nopause', 'devant',vitesse)
elif cote == 'gauche':
motor_A('nopause', 'devant', vitesse * 0.88)
motor_B('pause', 'devant', vitesse)
else:
motor_A('nopause', 'devant', vitesse * 0.88)
motor_B('nopause', 'devant', vitesse)
elif direction == 'derriere':
if cote == 'droit':
motor_A('pause', 'derriere', vitesse * 0.88)
motor_B('nopause', 'derriere', vitesse)
elif cote == 'gauche':
motor_A('nopause', 'derriere', vitesse * 0.88)
motor_B('pause', 'derriere', vitesse)
else:
motor_A('nopause', 'derriere', vitesse * 0.88)
motor_B('nopause', 'derriere', vitesse)
elif direction == 'no':
if cote == 'gauche':
motor_A('nopause', 'devant', vitesse * 0.88)
motor_B('nopause', 'arriere', vitesse)
elif cote == 'droit':
motor_A('nopause', 'derriere', vitesse * 0.88)
motor_B('nopause', 'devant', vitesse)
else:
motorStop()
else:
pass
def destroy():
GPIO.cleanup()
class Moteurs:
def __init__(self):
setup()
def avancer(self, vitesse):
conduite('devant', 'no', vitesse)
def tourner_droite(self, vitesse):
conduite('no', 'droit', vitesse)
def tourner_gauche(self, vitesse):
conduite('no', 'gauche', vitesse)
def reculer_droite(self, vitesse):
# Je dois identifier le moteur gauche et le moteur droit
conduite('derriere', 'droit', vitesse)
def reculer_gauche(self, vitesse):
conduite('derriere', 'gauche', vitesse)
def stop(self):
motorStop()
if __name__ == '__main__':
try:
vitesse = 50
robot = Moteurs()
#time.sleep(4)
#robot.stop()
#robot.tourner_droite(vitesse)
#time.sleep(4)
#robot.reculer(vitesse)
#time.sleep(4)
#robot.stop()
#time.sleep(4)
#robot.tourner_gauche(vitesse)
while True:
robot.avancer(vitesse)
except KeyboardInterrupt:
destroy()