Code
- include <Servo.h>
//broches servo
Servo servomoteur; int positionServomoteur = 0;
//broches trigger
int trigger = 2; int echo = 4; float distance;
int distanceGauche; int distanceDroite;
//broches moteur 1
int eMoteur1 = 11; int pin1Moteur1 =13; int pin2Moteur1 = 12;
//broches moteur 2
int eMoteur2 = 10; int pin1Moteur2 = 8; int pin2Moteur2 = 7;
void setup() {
pinMode(trigger, OUTPUT); //initialisation du trigger pinMode(echo, INPUT); servomoteur.attach(9); //initialisation du servomoteur servomoteur.write(positionServomoteur);
pinMode(eMoteur1, OUTPUT); //initialisation moteur 1 pinMode(pin1Moteur1, OUTPUT); pinMode(pin2Moteur1, OUTPUT); pinMode(eMoteur2, OUTPUT); //initialisation moteur 2 pinMode(pin1Moteur2, OUTPUT); pinMode(pin2Moteur2, OUTPUT);
}
void loop() {
servomoteur.write(90); CalculDistance();
if(distance>30)
{
servomoteur.write(90);
Roule();
}
if(distance<30)
{
Stop();
RegardeGauche();
delay(1000);
CalculDistance();
distanceGauche = distance;
RegardeDroite();
delay(1000);
CalculDistance();
distanceDroite = distance;
servomoteur.write(90);
if( (distanceGauche>distanceDroite) && (distanceGauche>20) )
{
TourneGauche();
delay(2000);
}
if( (distanceDroite>distanceGauche) && (distanceDroite>20) )
{
TourneDroite();
delay(2000);
}
else
{
Stop();
Recule();
delay(2000);
TourneGauche();
}
}
}
void RegardeGauche() //La voiture regarde à gauche
{
for(positionServomoteur = 90; positionServomoteur>=20; positionServomoteur--)
{
servomoteur.write(positionServomoteur); //servomoteur tourne à gauche
delay(15);
}
}
void RegardeDroite() //La voiture regarde à droite {
for(positionServomoteur = 90; positionServomoteur<=160; positionServomoteur++)
{
servomoteur.write(positionServomoteur); //sevomoteur tourne à droite
delay(15);
}
}
void CalculDistance() //La voiture calcule la distance qui la sépare d'un obstacle {
digitalWrite(trigger, 0); delayMicroseconds(2); digitalWrite(trigger, 1); delayMicroseconds(10); digitalWrite(trigger, 0); //envoi des impulsions distance = pulseIn(echo, 1) /58.0; //calcul de la distance
}
void Roule() //La voiture avance {
Moteur1(200, true); Moteur2(200, true);
}
void Stop() //La voiture s'arrète {
Moteur1(0, true); Moteur2(0, true);
}
void Recule() //La voiture recule {
Moteur1(150, false); Moteur2(150, false);
}
void TourneGauche() //La voiture tourne à gauche {
Moteur1(150, true); Moteur2(150, false);
}
void TourneDroite() //La voiture tourne à droite {
Moteur1(150, false); Moteur2(150, true);
}
void Moteur1(int speed, boolean reverse) //fonction moteur 1 {
analogWrite(eMoteur1, speed); digitalWrite(pin1Moteur1, ! reverse); digitalWrite(pin2Moteur1, reverse);
}
void Moteur2(int speed, boolean reverse) //fonction moteur 2 {
analogWrite(eMoteur2, speed); digitalWrite(pin1Moteur2, ! reverse); digitalWrite(pin2Moteur2, reverse);
}