TPE - Voitures autonomes

# 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)

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)

def avant_gauche(speed):
 print "Pivot Gauche"
 GPIO.output(motor2A,GPIO.HIGH)
 GPIO.output(motor2B,GPIO.LOW)
 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)
 motor2.ChangeDutyCycle(speed)

def stop():
 motor1.ChangeDutyCycle(0)
 motor2.ChangeDutyCycle(0)

# Boucle du programme pour lire les capteurs suiveurs de ligne et utiliser les 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) and (capteurCentre==GPIO.LOW):
   print "blanc a gauche // blanc a droite // noir au centre"
   avant(75)
  if (capteurGauche==GPIO.LOW):
   print "noir a gauche"
   stop()
   pivot_gauche(30)
  if (capteurDroite==GPIO.LOW):
   print "noir a droite"
   stop()
   pivot_droite(30)
  if (capteurGauche==GPIO.HIGH) and (capteurDroite==GPIO.HIGH) and (capteurCentre==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 suivante
        Reinitialisation()