TPE - Voitures autonomes
Le Raspi Car : une voiture autonome.
​
Pour achever notre production et pour qu'elle soit la plus complète possible, nous avons créé une grande route avec des virages et une ligne de différentes couleurs. Nous avons donc peint sur une nappe blanche un grand circuit.
​
​
Une fois la peinture sèche et le circuit fini, la création des programmes s'est faite en 2 parties. D'une part, nous avons repris le programme pour suivre une ligne mais cette fois avec seulement 2 capteurs suiveurs de ligne et nous l'avons perfectionner pour que la voiture ne sorte pas du circuit.
​
Voici le plan de câblage pour le premier programme :
​
sudo nano circuit_test.py
# Importation des librairies pour controle du port GPIO
import RPi.GPIO as GPIO
from time import sleep
# Acces au port GPIO avec la numerotation Broadcom
mode = GPIO.BCM
# Declaration des emplacements
capteurC = 3
capteurG = 14
capteurD = 15
motor1A=16
motor1B=20
motor1E=21
motor2A=9
motor2B=10
motor2E=11
GPIO.setmode(mode)
GPIO.setwarnings(False)
GPIO.setup(motor1E,GPIO.OUT)
GPIO.setup(motor2E,GPIO.OUT)
motor1 = GPIO.PWM(motor1E,50)
motor2 = GPIO.PWM(motor2E,50)
# Parametrage
def Parametrage():
GPIO.setup(capteurG, GPIO.IN, pull_up_down = GPIO.PUD_UP) #initialisation
GPIO.setup(capteurD, GPIO.IN, pull_up_down = GPIO.PUD_UP)
# GPIO.setup(capteurC, GPIO.IN, pull_up_down = GPIO.PUD_UP)
GPIO.setup(motor1A,GPIO.OUT)
GPIO.setup(motor1B,GPIO.OUT)
GPIO.setup(motor2A,GPIO.OUT)
GPIO.setup(motor2B,GPIO.OUT)
motor1.start(0)
motor2.start(0)
def avant(speed):
print "Avant"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
motor1.ChangeDutyCycle(speed)
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor2.ChangeDutyCycle(speed)
def arriere(speed):
print "Arriere"
GPIO.output(motor1A,GPIO.LOW)
GPIO.output(motor1B,GPIO.HIGH)
motor1.ChangeDutyCycle(speed)
GPIO.output(motor2A,GPIO.LOW)
GPIO.output(motor2B,GPIO.HIGH)
motor2.ChangeDutyCycle(speed)
def avant_droite(speed):
print "Pivot Droite"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
motor1.ChangeDutyCycle(speed)
motor2.ChangeDutyCycle(0)
def pivot_droite(speed):
print "Pivot Droite"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
GPIO.output(motor2A,GPIO.LOW)
GPIO.output(motor2B,GPIO.HIGH)
motor1.ChangeDutyCycle(speed)
motor2.ChangeDutyCycle(speed/3)
def avant_gauche(speed):
print "Pivot Gauche"
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor1.ChangeDutyCycle(0)
motor2.ChangeDutyCycle(speed)
def pivot_gauche(speed):
print "Pivot Gauche"
GPIO.output(motor1A,GPIO.LOW)
GPIO.output(motor1B,GPIO.HIGH)
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor1.ChangeDutyCycle(speed/3)
motor2.ChangeDutyCycle(speed)
def stop():
motor1.ChangeDutyCycle(0)
motor2.ChangeDutyCycle(0)
# Boucle du programme pour lire les 2 capteurs suiveurs de ligne et utiliser les fonctions des moteurs
def Boucle():
while True:
# capteurCentre=GPIO.input(capteurC)
capteurGauche=GPIO.input(capteurG)
capteurDroite=GPIO.input(capteurD)
print "---------------------------------------"
if (capteurGauche==GPIO.HIGH) and (capteurDroite==GPIO.HIGH):
print "blanc a gauche // blanc a droite"
avant(100)
sleep(0.005)
if (capteurGauche==GPIO.LOW):
print "noir a gauche"
pivot_gauche(60)
sleep(0.0001)
if (capteurDroite==GPIO.LOW):
print "noir a droite"
pivot_droite(60)
sleep(0.0001)
# if (capteurGauche==GPIO.HIGH) and (capteurDroite==GPIO.HIGH):
# print "blanc a gauche // blanc a droite // blanc au centre"
# stop()
# sleep(0.05)
def Reinitialisation(): # Reinitialiser le circuit GPIO
#GPIO.setup(capteurG, GPIO.LOW)
#GPIO.setup(capteurD, GPIO.LOW)
GPIO.cleanup() # Relacher les ressources
# Le programme demarre ici
if __name__ == '__main__':
Parametrage()
try:
Boucle()
except KeyboardInterrupt: # Si on interrompt le programme, on passe d'abord par la fonction reinitialisation
Reinitialisation()
​
Nous sommes satisfaits des résultats obtenus à la première partie :
​
​
Pour la deuxième partie, nous avons donc rajouté le capteur de couleur à notre programme :
​
sudo nano circuit_couleur.py
# Importation des librairies pour controle du port GPIO
import RPi.GPIO as GPIO
import time
import threading
import pigpio
import sys
# Definition de la classe du capteur de couleur
class sensor(threading.Thread):
"""
This class reads RGB values from a TCS3200 colour sensor.
GND Ground.
VDD Supply Voltage (2.7-5.5V)
/OE Output enable, active low. When OE is high OUT is disabled
allowing multiple sensors to share the same OUT line.
OUT Output frequency square wave.
S0/S1 Output frequency scale selection.
S2/S3 Colour filter selection.
OUT is a square wave whose frequency is proprtional to the
intensity of the selected filter colour.
S2/S3 selects between red, green, blue, and no filter.
S0/S1 scales the frequency at 100%, 20%, 2% or off.
To take a reading the colour filters are selected in turn for a
fraction of a second and the frequency is read and converted to
Hz.
"""
def __init__(self, pi, OUT, S2, S3, S0=None, S1=None, OE=None):
"""
The gpios connected to the sensor OUT, S2, and S3 pins must
be specified. The S2, S3 (frequency) and OE (output enable)
gpios are optional.
"""
threading.Thread.__init__(self)
self._pi = pi
self._OUT = OUT
self._S2 = S2
self._S3 = S3
self._mode_OUT = pi.get_mode(OUT)
self._mode_S2 = pi.get_mode(S2)
self._mode_S3 = pi.get_mode(S3)
pi.write(OUT, 0) # disable output gpio
pi.set_mode(S2, pigpio.OUTPUT)
pi.set_mode(S3, pigpio.OUTPUT)
self._S0 = S0
self._S1 = S1
self._OE = OE
if (S0 is not None) and (S1 is not None):
self._mode_S0 = pi.get_mode(S0)
self._mode_S1 = pi.get_mode(S1)
pi.set_mode(S0, pigpio.OUTPUT)
pi.set_mode(S1, pigpio.OUTPUT)
if OE is not None:
self._mode_OE = pi.get_mode(OE)
pi.set_mode(OE, pigpio.OUTPUT)
pi.write(OE, 0) # enable device
self.set_sample_size(50)
self._period = 0.25 # 4 readings per second
self.set_frequency(1) # 2%
self._rgb_black = [0]*3
self._rgb_white = [10000]*3
self._set_filter(3) # Clear
self.Hertz=[0]*3 # latest triplet
self._Hertz=[0]*3 # current values
self.tally=[1]*3 # latest triplet
self._tally=[1]*3 # current values
self._delay=[0.1]*3 # tune delay to get TALLY readings
self._edge = 0
self._start_tick = 0
self._last_tick = 0
self._cb_OUT = pi.callback(OUT, pigpio.RISING_EDGE, self._cbf)
self._cb_S2 = pi.callback(S2, pigpio.EITHER_EDGE, self._cbf)
self._cb_S3 = pi.callback(S3, pigpio.EITHER_EDGE, self._cbf)
self._read = True
self.daemon = True
self.start()
def _cbf(self, g, l, t):
if g == self._OUT:
if self._edge == 0:
self._start_tick = t
else:
self._last_tick = t
self._edge += 1
else: # Must be transition between colour samples
if g == self._S2:
if l == 0: # Clear -> Red
self._edge = 0
return
else: # Blue -> Green
colour = 2
else:
if l == 0: # Green -> Clear
colour = 1
else: # Red -> Blue
colour = 0
if self._edge > 1:
self._edge -= 1
td = pigpio.tickDiff(self._start_tick, self._last_tick)
self._Hertz[colour] = (1000000 * self._edge) / td
self._tally[colour] = self._edge
else:
self._Hertz[colour] = 0
self._tally[colour] = 0
self._edge = 0
# Have we a new set of RGB?
if colour == 1:
for i in range(3):
self.Hertz[i] = self._Hertz[i]
self.tally[i] = self._tally[i]
def run(self):
while True:
if self._read:
next_time = time.time() + self._period
self._pi.set_mode(self._OUT, pigpio.INPUT) # enable output gpio
# The order Red -> Blue -> Green -> Clear is needed by the
# callback function so that each S2/S3 transition triggers
# a state change. The order was chosen so that a single
# gpio changes state between the change in colour to be
# sampled.
self._set_filter(0) # Red
time.sleep(self._delay[0])
self._set_filter(2) # Blue
time.sleep(self._delay[2])
self._set_filter(1) # Green
time.sleep(self._delay[1])
self._pi.write(self._OUT, 0) # disable output gpio
self._set_filter(3) # Clear
delay = next_time - time.time()
if delay > 0.0:
time.sleep(delay)
# Tune the next set of delays to get reasonable results
# as quickly as possible.
for c in range(3):
# Calculate dly needed to get a decent number of samples
if self.Hertz[c]:
dly = self._samples / float(self.Hertz[c])
# Constrain dly to reasonable values
if dly < 0.001:
dly = 0.001
elif dly > 0.5:
dly = 0.5
self._delay[c] = dly
else:
time.sleep(0.1)
def pause(self):
"""
No more readings will be made until resume is called.
"""
self._read = False
def resume(self):
"""
Resumes readings (after a call to pause).
"""
self._read = True
def get_Hertz(self):
"""
Returns the latest Hertz reading.
"""
return self.Hertz
def get_rgb(self, top=255):
"""
Returns the latest RGB reading.
The raw colour Hertz readings are converted to RGB values.
By default the RGB values are constrained to be between
0 and 255. A different upper limit can be set by using
the top parameter.
"""
rgb = [0]*3
for c in range(3):
v = self.Hertz[c] - self._rgb_black[c]
s = self._rgb_white[c] - self._rgb_black[c]
p = top * v / s
if p < 0:
p = 0
elif p > top:
p = top
rgb[c] = p
return rgb
def cancel(self):
"""
Cancels the sensor and release all used resources.
"""
self._cb_S3.cancel()
self._cb_S2.cancel()
self._cb_OUT.cancel()
self.set_frequency(0) # off
self._set_filter(3) # Clear
self._pi.set_mode(self._OUT, self._mode_OUT)
self._pi.set_mode(self._S2, self._mode_S2)
self._pi.set_mode(self._S3, self._mode_S3)
if (self._S0 is not None) and (self._S1 is not None):
self._pi.set_mode(self._S0, self._mode_S0)
self._pi.set_mode(self._S1, self._mode_S1)
if self._OE is not None:
self._pi.write(self._OE, 1) # disable device
self._pi.set_mode(self._OE, self._mode_OE)
def set_black_level(self, rgb):
"""
Sets the black level calibration.
"""
for i in range(3):
self._rgb_black[i] = rgb[i]
def set_white_level(self, rgb):
"""
Sets the white level calibration.
"""
for i in range(3):
self._rgb_white[i] = rgb[i]
def _set_filter(self, f):
"""
Set the colour to be sampled.
f S2 S3 Photodiode
0 L L Red
1 H H Green
2 L H Blue
3 H L Clear (no filter)
"""
if f == 0: # Red
S2 = 0; S3 = 0
elif f == 1: # Green
S2 = 1; S3 = 1
elif f == 2: # Blue
S2 = 0; S3 = 1
else: # Clear
S2 = 1; S3 = 0
self._pi.write(self._S2, S2); self._pi.write(self._S3, S3)
def get_frequency(self):
"""
Returns the current frequency scaling.
"""
return self._frequency
def set_frequency(self, f):
"""
Sets the frequency scaling.
f S0 S1 Frequency scaling
0 L L Off
1 L H 2%
2 H L 20%
3 H H 100%
"""
if f == 0: # off
S0 = 0; S1 = 0
elif f == 1: # 2%
S0 = 0; S1 = 1
elif f == 2: # 20%
S0 = 1; S1 = 0
else: # 100%
S0 = 1; S1 = 1
if (self._S0 is not None) and (self._S1 is not None):
self._frequency = f
self._pi.write(self._S0, S0)
self._pi.write(self._S1, S1)
else:
self._frequency = None
def set_update_period(self, t):
"""
Sets the period between RGB updates.
"""
if (t >= 0.1) and (t < 2.0):
self._period = t
def set_sample_size(self, samples):
if (samples < 10) or (samples > 1000):
samples = 50
self._samples = samples
# Acces au port GPIO avec la numerotation Broadcom
mode = GPIO.BCM
# Declaration des emplacements
#capteurC = 3
capteurG = 14
capteurD = 15
motor1A=16
motor1B=20
motor1E=21
motor2A=9
motor2B=10
motor2E=11
GPIO.setmode(mode)
GPIO.setwarnings(False)
GPIO.setup(motor1E,GPIO.OUT)
GPIO.setup(motor2E,GPIO.OUT)
motor1 = GPIO.PWM(motor1E,50)
motor2 = GPIO.PWM(motor2E,50)
vitesse = 75
RED=12
GREEN=13
BLUE=19
pi = pigpio.pi()
p = 0.333
# Bottom view
#
# GND VCC
# OE OUT
# S1 S2
# S0 S3
s = sensor(pi, 24, 22, 23, 4, 17, 18)
s.set_update_period(p)
s.set_frequency(2) # 20%
s.set_sample_size(20)
# Parametrage
def Parametrage():
GPIO.setup(capteurG, GPIO.IN, pull_up_down = GPIO.PUD_UP) #initialisation
GPIO.setup(capteurD, GPIO.IN, pull_up_down = GPIO.PUD_UP)
# GPIO.setup(capteurC, GPIO.IN, pull_up_down = GPIO.PUD_UP)
GPIO.setup(motor1A,GPIO.OUT)
GPIO.setup(motor1B,GPIO.OUT)
GPIO.setup(motor2A,GPIO.OUT)
GPIO.setup(motor2B,GPIO.OUT)
motor1.start(0)
motor2.start(0)
# Calibrage
def Calibrage():
wait_for_return("Calibrage du noir, appuyer sur Entree pour demarrer")
for i in range(5):
time.sleep(p)
rgb = s.get_Hertz()
print(rgb)
s.set_black_level(rgb)
wait_for_return("Calibrage du blanc, appuyer sur Entree pour demarrer")
for i in range(5):
time.sleep(p)
rgb = s.get_Hertz()
print(rgb)
s.set_white_level(rgb)
wait_for_return("Position sur la ligne de depart, appuyer sur Entree pour demarrer")
def avant(speed):
print "Avant"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
motor1.ChangeDutyCycle(speed)
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor2.ChangeDutyCycle(speed)
def arriere(speed):
print "Arriere"
GPIO.output(motor1A,GPIO.LOW)
GPIO.output(motor1B,GPIO.HIGH)
motor1.ChangeDutyCycle(speed)
GPIO.output(motor2A,GPIO.LOW)
GPIO.output(motor2B,GPIO.HIGH)
motor2.ChangeDutyCycle(speed)
def avant_droite(speed):
print "Pivot Droite"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
motor1.ChangeDutyCycle(speed)
motor2.ChangeDutyCycle(O)
def pivot_droite(speed):
print "Pivot Droite"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
GPIO.output(motor2A,GPIO.LOW)
GPIO.output(motor2B,GPIO.HIGH)
motor1.ChangeDutyCycle(speed)
motor2.ChangeDutyCycle(speed/2)
def avant_gauche(speed):
print "Pivot Gauche"
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor1.ChangeDutyCycle(0)
motor2.ChangeDutyCycle(speed)
def pivot_gauche(speed):
print "Pivot Gauche"
GPIO.output(motor1A,GPIO.LOW)
GPIO.output(motor1B,GPIO.HIGH)
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor1.ChangeDutyCycle(speed/2)
motor2.ChangeDutyCycle(speed)
def stop():
motor1.ChangeDutyCycle(0)
motor2.ChangeDutyCycle(0)
# Boucle du programme pour lire les 2 capteurs suiveurs de ligne et le capteur de couleur pour utiliser les moteurs
def Boucle():
vitesse=70
while True:
# time.sleep(p)
rgb = s.get_rgb()
r = rgb[0]
g = rgb[1]
b = rgb[2]
print(rgb, s.get_Hertz(), s.tally)
pi.set_PWM_dutycycle(RED, r)
pi.set_PWM_dutycycle(GREEN, g)
pi.set_PWM_dutycycle(BLUE, b)
if (r>=g+10) and (r>=b+10):
print "rouge"
vitesse=100
if (g>=r+10) and (g>=b+10):
print "vert"
vitesse=80
if (b>=r+10) and (b>=g+10):
print "bleu"
vitesse=90
if (r>=240) and (g>=240) and (b>=240): print "blanc"
if (r<=15) and (g<=15) and (b<=15): print "noir"
# capteurCentre=GPIO.input(capteurC)
capteurGauche=GPIO.input(capteurG)
capteurDroite=GPIO.input(capteurD)
print "---------------------------------------"
# if (capteurGauche==GPIO.HIGH) and (capteurDroite==GPIO.HIGH) and (capteurCentre==GPIO.LOW):
if (capteurGauche==GPIO.HIGH) and (capteurDroite==GPIO.HIGH):
# print "blanc a gauche // blanc a droite // noir au centre"
print "blanc a gauche // blanc a droite"
avant(vitesse)
time.sleep(0.005)
if (capteurGauche==GPIO.LOW):
print "noir a gauche"
pivot_gauche(80)
time.sleep(0.0001)
if (capteurDroite==GPIO.LOW):
print "noir a droite"
pivot_droite(80)
time.sleep(0.0001)
# if (capteurGauche==GPIO.HIGH) and (capteurDroite==GPIO.HIGH) and (capteurCentre==GPIO.HIGH):
# print "blanc a gauche // blanc a droite // blanc au centre"
# stop()
def Reinitialisation(): # Reinitialiser le circuit GPIO
#GPIO.setup(capteurG, GPIO.LOW)
#GPIO.setup(capteurD, GPIO.LOW)
GPIO.cleanup() # Relacher les ressources
def wait_for_return(str):
if sys.hexversion < 0x03000000:
raw_input(str)
else:
input(str)
# Le programme demarre ici
if __name__ == '__main__':
Parametrage()
Calibrage()
try:
Boucle()
except KeyboardInterrupt: # Si on interrompt le programme, on passe d'abord par la fonction Reinitialisation
Reinitialisation()
​
Après de nombreuses difficultés, ce dernier programme fonctionne très bien, la preuve en vidéo :
​
​
Nous pouvons voir que le Raspi Car est plus rapide sur la ligne rouge tandis que lorsque la ligne est verte en virage, il ralentit. En effet, c'est grâce à l'intelligence artificielle que le Raspberry prend la décision d'accélérer ou de ralentir en fonction de ce que le capteur de couleur a analysé.
​
Nous avons décidé de développer encore plus notre production et nous avons créé un programme utilisant tous les composants électroniques que nous possédons. Notre Raspi Car sera donc capable d'adapter sa vitesse à la couleur de la ligne qu'il suit mais il pourra aussi allumer sa LED lorsqu'il fait trop sombre. De plus, notre voiture autonome sera capable de s'arrêter lors de la présence d'un obstacle sur sa route et de regarder autour de lui afin de savoir si l'obstacle le bloque.
Pour ce faire, notre plan de câblage est donc le plan de câblage complet ci-dessous :
​
​
Notre programme est donc très conséquent et nous avons dû réaliser de nombreux tests avant de réussir toutes les tâches demandées.
​
sudo nano circuit_complet.py
# Importation des librairies pour controle du port GPIO
import RPi.GPIO as GPIO
import time
import threading
import pigpio
import sys
# Definition de la classe du capteur de couleur
class sensor(threading.Thread):
"""
This class reads RGB values from a TCS3200 colour sensor.
GND Ground.
VDD Supply Voltage (2.7-5.5V)
/OE Output enable, active low. When OE is high OUT is disabled
allowing multiple sensors to share the same OUT line.
OUT Output frequency square wave.
S0/S1 Output frequency scale selection.
S2/S3 Colour filter selection.
OUT is a square wave whose frequency is proprtional to the
intensity of the selected filter colour.
S2/S3 selects between red, green, blue, and no filter.
S0/S1 scales the frequency at 100%, 20%, 2% or off.
To take a reading the colour filters are selected in turn for a
fraction of a second and the frequency is read and converted to
Hz.
"""
def __init__(self, pi, OUT, S2, S3, S0=None, S1=None, OE=None):
"""
The gpios connected to the sensor OUT, S2, and S3 pins must
be specified. The S2, S3 (frequency) and OE (output enable)
gpios are optional.
"""
threading.Thread.__init__(self)
self._pi = pi
self._OUT = OUT
self._S2 = S2
self._S3 = S3
self._mode_OUT = pi.get_mode(OUT)
self._mode_S2 = pi.get_mode(S2)
self._mode_S3 = pi.get_mode(S3)
pi.write(OUT, 0) # disable output gpio
pi.set_mode(S2, pigpio.OUTPUT)
pi.set_mode(S3, pigpio.OUTPUT)
self._S0 = S0
self._S1 = S1
self._OE = OE
if (S0 is not None) and (S1 is not None):
self._mode_S0 = pi.get_mode(S0)
self._mode_S1 = pi.get_mode(S1)
pi.set_mode(S0, pigpio.OUTPUT)
pi.set_mode(S1, pigpio.OUTPUT)
if OE is not None:
self._mode_OE = pi.get_mode(OE)
pi.set_mode(OE, pigpio.OUTPUT)
pi.write(OE, 0) # enable device
self.set_sample_size(50)
self._period = 0.25 # 4 readings per second
self.set_frequency(1) # 2%
self._rgb_black = [0]*3
self._rgb_white = [10000]*3
self._set_filter(3) # Clear
self.Hertz=[0]*3 # latest triplet
self._Hertz=[0]*3 # current values
self.tally=[1]*3 # latest triplet
self._tally=[1]*3 # current values
self._delay=[0.1]*3 # tune delay to get TALLY readings
self._edge = 0
self._start_tick = 0
self._last_tick = 0
self._cb_OUT = pi.callback(OUT, pigpio.RISING_EDGE, self._cbf)
self._cb_S2 = pi.callback(S2, pigpio.EITHER_EDGE, self._cbf)
self._cb_S3 = pi.callback(S3, pigpio.EITHER_EDGE, self._cbf)
self._read = True
self.daemon = True
self.start()
def _cbf(self, g, l, t):
if g == self._OUT:
if self._edge == 0:
self._start_tick = t
else:
self._last_tick = t
self._edge += 1
else: # Must be transition between colour samples
if g == self._S2:
if l == 0: # Clear -> Red
self._edge = 0
return
else: # Blue -> Green
colour = 2
else:
if l == 0: # Green -> Clear
colour = 1
else: # Red -> Blue
colour = 0
if self._edge > 1:
self._edge -= 1
td = pigpio.tickDiff(self._start_tick, self._last_tick)
self._Hertz[colour] = (1000000 * self._edge) / td
self._tally[colour] = self._edge
else:
self._Hertz[colour] = 0
self._tally[colour] = 0
self._edge = 0
# Have we a new set of RGB?
if colour == 1:
for i in range(3):
self.Hertz[i] = self._Hertz[i]
self.tally[i] = self._tally[i]
def run(self):
while True:
if self._read:
next_time = time.time() + self._period
self._pi.set_mode(self._OUT, pigpio.INPUT) # enable output gpio
# The order Red -> Blue -> Green -> Clear is needed by the
# callback function so that each S2/S3 transition triggers
# a state change. The order was chosen so that a single
# gpio changes state between the change in colour to be
# sampled.
self._set_filter(0) # Red
time.sleep(self._delay[0])
self._set_filter(2) # Blue
time.sleep(self._delay[2])
self._set_filter(1) # Green
time.sleep(self._delay[1])
self._pi.write(self._OUT, 0) # disable output gpio
self._set_filter(3) # Clear
delay = next_time - time.time()
if delay > 0.0:
time.sleep(delay)
# Tune the next set of delays to get reasonable results
# as quickly as possible.
for c in range(3):
# Calculate dly needed to get a decent number of samples
if self.Hertz[c]:
dly = self._samples / float(self.Hertz[c])
# Constrain dly to reasonable values
if dly < 0.001:
dly = 0.001
elif dly > 0.5:
dly = 0.5
self._delay[c] = dly
else:
time.sleep(0.1)
def pause(self):
"""
No more readings will be made until resume is called.
"""
self._read = False
def resume(self):
"""
Resumes readings (after a call to pause).
"""
self._read = True
def get_Hertz(self):
"""
Returns the latest Hertz reading.
"""
return self.Hertz
def get_rgb(self, top=255):
"""
Returns the latest RGB reading.
The raw colour Hertz readings are converted to RGB values.
By default the RGB values are constrained to be between
0 and 255. A different upper limit can be set by using
the top parameter.
"""
rgb = [0]*3
for c in range(3):
v = self.Hertz[c] - self._rgb_black[c]
s = self._rgb_white[c] - self._rgb_black[c]
p = top * v / s
if p < 0:
p = 0
elif p > top:
p = top
rgb[c] = p
return rgb
def cancel(self):
"""
Cancels the sensor and release all used resources.
"""
self._cb_S3.cancel()
self._cb_S2.cancel()
self._cb_OUT.cancel()
self.set_frequency(0) # off
self._set_filter(3) # Clear
self._pi.set_mode(self._OUT, self._mode_OUT)
self._pi.set_mode(self._S2, self._mode_S2)
self._pi.set_mode(self._S3, self._mode_S3)
if (self._S0 is not None) and (self._S1 is not None):
self._pi.set_mode(self._S0, self._mode_S0)
self._pi.set_mode(self._S1, self._mode_S1)
if self._OE is not None:
self._pi.write(self._OE, 1) # disable device
self._pi.set_mode(self._OE, self._mode_OE)
def set_black_level(self, rgb):
"""
Sets the black level calibration.
"""
for i in range(3):
self._rgb_black[i] = rgb[i]
def set_white_level(self, rgb):
"""
Sets the white level calibration.
"""
for i in range(3):
self._rgb_white[i] = rgb[i]
def _set_filter(self, f):
"""
Set the colour to be sampled.
f S2 S3 Photodiode
0 L L Red
1 H H Green
2 L H Blue
3 H L Clear (no filter)
"""
if f == 0: # Red
S2 = 0; S3 = 0
elif f == 1: # Green
S2 = 1; S3 = 1
elif f == 2: # Blue
S2 = 0; S3 = 1
else: # Clear
S2 = 1; S3 = 0
self._pi.write(self._S2, S2); self._pi.write(self._S3, S3)
def get_frequency(self):
"""
Returns the current frequency scaling.
"""
return self._frequency
def set_frequency(self, f):
"""
Sets the frequency scaling.
f S0 S1 Frequency scaling
0 L L Off
1 L H 2%
2 H L 20%
3 H H 100%
"""
if f == 0: # off
S0 = 0; S1 = 0
elif f == 1: # 2%
S0 = 0; S1 = 1
elif f == 2: # 20%
S0 = 1; S1 = 0
else: # 100%
S0 = 1; S1 = 1
if (self._S0 is not None) and (self._S1 is not None):
self._frequency = f
self._pi.write(self._S0, S0)
self._pi.write(self._S1, S1)
else:
self._frequency = None
def set_update_period(self, t):
"""
Sets the period between RGB updates.
"""
if (t >= 0.1) and (t < 2.0):
self._period = t
def set_sample_size(self, samples):
if (samples < 10) or (samples > 1000):
samples = 50
self._samples = samples
# Acces au port GPIO avec la numerotation Broadcom
mode = GPIO.BCM
# Declaration des emplacements
capteurG = 14
capteurD = 15
motor1A=16
motor1B=20
motor1E=21
motor2A=9
motor2B=10
motor2E=11
GPIO.setmode(mode)
GPIO.setwarnings(False)
GPIO.setup(motor1E,GPIO.OUT)
GPIO.setup(motor2E,GPIO.OUT)
motor1 = GPIO.PWM(motor1E,50)
motor2 = GPIO.PWM(motor2E,50)
vitesse = 80
TRIG = 7
ECHO = 8
pan = 5
Tilt = 6
GPIO.setup(pan, GPIO.OUT)
GPIO.setup(Tilt, GPIO.OUT)
ajoutAngle = 5
ldr = 26
led = 27
GPIO.setup(led,GPIO.OUT)
RED=12
GREEN=13
BLUE=19
pi = pigpio.pi()
p = 0.333
# Bottom view
#
# GND VCC
# OE OUT
# S1 S2
# S0 S3
s = sensor(pi, 24, 22, 23, 4, 17, 18)
s.set_update_period(p)
s.set_frequency(2) # 20%
s.set_sample_size(20)
# Parametrage
def Parametrage():
GPIO.setup(capteurG, GPIO.IN, pull_up_down = GPIO.PUD_UP) #initialisation
GPIO.setup(capteurD, GPIO.IN, pull_up_down = GPIO.PUD_UP)
GPIO.setup(motor1A,GPIO.OUT)
GPIO.setup(motor1B,GPIO.OUT)
GPIO.setup(motor2A,GPIO.OUT)
GPIO.setup(motor2B,GPIO.OUT)
motor1.start(0)
motor2.start(0)
# Calibrage
def Calibrage():
wait_for_return("Calibrage du noir, appuyer sur Entree pour demarrer")
for i in range(5):
time.sleep(p)
rgb = s.get_Hertz()
print(rgb)
s.set_black_level(rgb)
wait_for_return("Calibrage du blanc, appuyer sur Entree pour demarrer")
for i in range(5):
time.sleep(p)
rgb = s.get_Hertz()
print(rgb)
s.set_white_level(rgb)
wait_for_return("Position sur la ligne de depart, appuyer sur Entree pour demarrer")
def avant(speed):
# print "Avant"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
motor1.ChangeDutyCycle(speed)
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor2.ChangeDutyCycle(speed)
def arriere(speed):
# print "Arriere"
GPIO.output(motor1A,GPIO.LOW)
GPIO.output(motor1B,GPIO.HIGH)
motor1.ChangeDutyCycle(speed)
GPIO.output(motor2A,GPIO.LOW)
GPIO.output(motor2B,GPIO.HIGH)
motor2.ChangeDutyCycle(speed)
def avant_droite(speed):
# print "Pivot Droite"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
motor1.ChangeDutyCycle(speed)
motor2.ChangeDutyCycle(O)
def pivot_droite(speed):
# print "Pivot Droite"
GPIO.output(motor1A,GPIO.HIGH)
GPIO.output(motor1B,GPIO.LOW)
GPIO.output(motor2A,GPIO.LOW)
GPIO.output(motor2B,GPIO.HIGH)
motor1.ChangeDutyCycle(speed)
motor2.ChangeDutyCycle(speed/2)
def avant_gauche(speed):
# print "Pivot Gauche"
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor1.ChangeDutyCycle(0)
motor2.ChangeDutyCycle(speed)
def pivot_gauche(speed):
# print "Pivot Gauche"
GPIO.output(motor1A,GPIO.LOW)
GPIO.output(motor1B,GPIO.HIGH)
GPIO.output(motor2A,GPIO.HIGH)
GPIO.output(motor2B,GPIO.LOW)
motor1.ChangeDutyCycle(speed/2)
motor2.ChangeDutyCycle(speed)
def stop():
motor1.ChangeDutyCycle(0)
motor2.ChangeDutyCycle(0)
def regarde_gauche():
angle = 170
pwm=GPIO.PWM(pan,100)
pwm.start(5)
angleChoisi = float(angle)/10 + ajoutAngle
pwm.ChangeDutyCycle(angleChoisi)
time.sleep(0.1)
pwm.stop()
time.sleep(0.9)
def regarde_droite():
angle = 110
pwm=GPIO.PWM(pan,100)
pwm.start(5)
angleChoisi = float(angle)/10 + ajoutAngle
pwm.ChangeDutyCycle(angleChoisi)
time.sleep(0.1)
pwm.stop()
time.sleep(0.9)
def regarde_haut():
angle = 90
pwm=GPIO.PWM(Tilt,100)
pwm.start(5)
angleChoisi = float(angle)/10 + ajoutAngle
pwm.ChangeDutyCycle(angleChoisi)
time.sleep(0.1)
pwm.stop()
time.sleep(0.9)
def regarde_bas():
angle = 140
pwm=GPIO.PWM(Tilt,100)
pwm.start(5)
angleChoisi = float(angle)/10 + ajoutAngle
pwm.ChangeDutyCycle(angleChoisi)
time.sleep(0.1)
pwm.stop()
time.sleep(0.9)
def pan_regarde_devant():
angle = 130
pwm=GPIO.PWM(pan,100)
pwm.start(5)
angleChoisi = float(angle)/10 + ajoutAngle
pwm.ChangeDutyCycle(angleChoisi)
time.sleep(0.1)
pwm.stop()
time.sleep(0.9)
def tilt_regarde_devant():
angle = 115
pwm=GPIO.PWM(Tilt,100)
pwm.start(5)
angleChoisi = float(angle)/10 + ajoutAngle
pwm.ChangeDutyCycle(angleChoisi)
time.sleep(0.1)
pwm.stop()
time.sleep(0.9)
def verification_obstacle():
regarde_gauche()
regarde_droite()
pan_regarde_devant()
regarde_haut()
regarde_bas()
tilt_regarde_devant()
def distance():
#Capteur ultrason
pulse_start=0
pulse_end=0
GPIO.setup(TRIG,GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.output(TRIG,False)
#print "Attente"
time.sleep(0.05)
GPIO.output(TRIG,True)
time.sleep(0.00001)
GPIO.output(TRIG,False)
while GPIO.input(ECHO)==0 :
pulse_start = time.time()
while GPIO.input(ECHO)==1 :
pulse_end = time.time()
pulse_duration = pulse_end - pulse_start
d = pulse_duration * 17150
d = round(d,2)
return d
def niveau_lumiere():
valeur=0
GPIO.setup(ldr, GPIO.OUT)
GPIO.output(ldr, GPIO.LOW)
time.sleep(0.0001)
GPIO.setup(ldr, GPIO.IN)
while (GPIO.input(ldr) == GPIO.LOW):
valeur += 1
return valeur
def avance():
capteurGauche=GPIO.input(capteurG)
capteurDroite=GPIO.input(capteurD)
if (capteurGauche==GPIO.HIGH) and (capteurDroite==GPIO.HIGH):
# print "blanc a gauche // blanc a droite // noir au centre"
# print "blanc a gauche // blanc a droite"
avant(vitesse)
time.sleep(0.005)
if (capteurGauche==GPIO.LOW):
# print "noir a gauche"
pivot_gauche(80)
time.sleep(0.0001)
if (capteurDroite==GPIO.LOW):
# print "noir a droite"
pivot_droite(80)
time.sleep(0.0001)
# Boucle du programme pour lire tous les capteurs et utiliser les moteurs
def Boucle():
vitesse=70
d = 100
while True:
# time.sleep(p)
rgb = s.get_rgb()
r = rgb[0]
g = rgb[1]
b = rgb[2]
# print(rgb, s.get_Hertz(), s.tally)
pi.set_PWM_dutycycle(RED, r)
pi.set_PWM_dutycycle(GREEN, g)
pi.set_PWM_dutycycle(BLUE, b)
if (r>=g+10) and (r>=b+10):
# print "rouge"
vitesse=100
if (g>=r+10) and (g>=b+10):
# print "vert"
vitesse=80
if (b>=r+10) and (b>=g+10):
# print "bleu"
vitesse=90
# if (r>=240) and (g>=240) and (b>=240): print "blanc"
# if (r<=15) and (g<=15) and (b<=15): print "noir"
lum = niveau_lumiere()
print "---------------------------------------"
if (lum > 1000):
GPIO.output(led,True)
else:
GPIO.output(led,False)
print "lumiere : ", lum
d = distance()
print "distance %.1f" %d
if (d < 15):
stop()
verification_obstacle()
else:
avance()
def Reinitialisation(): # Eteindre la LED et reinitialiser le circuit GPIO
GPIO.output(led,GPIO.LOW)
GPIO.cleanup() # Relacher les ressources
def wait_for_return(str):
if sys.hexversion < 0x03000000:
raw_input(str)
else:
input(str)
# Le programme demarre ici
if __name__ == '__main__':
Parametrage()
Calibrage()
try:
Boucle()
except KeyboardInterrupt: # Si on interrompt le programme, on passe d'abord par la fonction Reinitialisation
Reinitialisation()
# tests realises ci dessous
def test1():
regarde_gauche()
regarde_droite()
pan_regarde_devant()
d = distance()
print "distance %.1f" %d
def test2():
regarde_haut()
regarde_bas()
tilt_regarde_devant()
d = distance()
print "distance %.1f" %d
def test3():
d = distance()
print "distance %.1f" %d
if (d < 15):
verification_obstacle()
def test4():
lum = niveau_lumiere()
if (lum > 1000):
GPIO.output(led,True)
time.sleep(1)
else:
GPIO.output(led,False)
print "lumiere : ", lum
#test1()
#test2()
#test3()
#test4()
#GPIO.cleanup()
​
L’exécution de ce programme s'est réalisée dans la vidéo ci-dessous. Ce sera la dernière vidéo de notre Raspi Car et c'est sûrement la plus aboutie. Nous avons donc atteint notre but : les prémices d'une voiture autonome.
​