132 lines
2.5 KiB
C++
132 lines
2.5 KiB
C++
// ; Pins capteurs de couleur
|
|
#define DROITE A0
|
|
#define GAUCHE A1
|
|
|
|
|
|
|
|
|
|
// Pins carte de commande (moteur n°1)
|
|
#define PWM_GAUCHE 5
|
|
#define SENS_GAUCHE 4
|
|
// (moteur n°2)
|
|
#define PWM_DROIT 6
|
|
#define SENS_DROIT 7
|
|
|
|
#define SIZE 60
|
|
|
|
unsigned int droite;
|
|
unsigned int gauche;
|
|
unsigned int cpt;
|
|
|
|
unsigned int queue_droite[SIZE];
|
|
unsigned int queue_gauche[SIZE];
|
|
|
|
unsigned int c= 0;
|
|
|
|
void setup() {
|
|
|
|
|
|
cpt = 0;
|
|
|
|
|
|
// Capteur
|
|
pinMode(GAUCHE, INPUT);
|
|
pinMode(DROITE, INPUT);
|
|
|
|
|
|
// Carte Commande
|
|
pinMode(PWM_GAUCHE, OUTPUT);
|
|
pinMode(SENS_GAUCHE, OUTPUT);
|
|
pinMode(PWM_DROIT, OUTPUT);
|
|
pinMode(SENS_DROIT, OUTPUT);
|
|
|
|
|
|
// Affichage série
|
|
Serial.begin(115200);
|
|
}
|
|
|
|
|
|
void loop() {
|
|
c++;
|
|
// Lecture capteur de couleur
|
|
gauche = analogRead(GAUCHE);
|
|
droite = analogRead(DROITE);
|
|
|
|
/* Zone noires, avancer
|
|
* Le robot est sur la ligne
|
|
*/
|
|
if(gauche > 700 && droite > 700)
|
|
{
|
|
vitesseGauche(180);
|
|
vitesseDroit(180);
|
|
}
|
|
|
|
|
|
/* Capteur gauche --> blanc : tourner à droite
|
|
* Le robot est à gauche de la ligne
|
|
*/
|
|
if(gauche < 200 && droite > 700)
|
|
{
|
|
vitesseGauche(150*0.9);
|
|
vitesseDroit(-70);
|
|
}
|
|
|
|
|
|
/* Capteur droit --> blanc : tourner à gauche
|
|
* Le robot est à droite de la ligne
|
|
*/
|
|
if(gauche > 700 && droite < 200)
|
|
{
|
|
vitesseGauche(-70);
|
|
vitesseDroit(150);
|
|
}
|
|
|
|
|
|
/* Bleu : demi tour et arret=*/
|
|
if(gauche > 380 && gauche < 570 && droite > 380 && droite < 570)
|
|
{
|
|
if (cpt == 0) {
|
|
demiTour();
|
|
cpt = 1;
|
|
} else if (cpt == 1) {
|
|
vitesseDroit(0);
|
|
vitesseGauche(0);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
// Controle du sens et de la vitesse du moteur Gauche
|
|
void vitesseGauche(int vitesse) {
|
|
if (vitesse >= 0) {
|
|
digitalWrite(SENS_GAUCHE, HIGH);
|
|
analogWrite(PWM_GAUCHE, vitesse);
|
|
} else {
|
|
digitalWrite(SENS_GAUCHE, LOW);
|
|
analogWrite(PWM_GAUCHE, -vitesse);
|
|
}
|
|
}
|
|
|
|
|
|
// Controle du sens et de la vitesse du moteur Droit
|
|
void vitesseDroit(int vitesse) {
|
|
if (vitesse >= 0) {
|
|
digitalWrite(SENS_DROIT, HIGH);
|
|
analogWrite(PWM_DROIT, vitesse);
|
|
} else {
|
|
digitalWrite(SENS_DROIT, LOW);
|
|
analogWrite(PWM_DROIT, -vitesse);
|
|
}
|
|
}
|
|
|
|
|
|
void demiTour() {
|
|
while(gauche < 700) {
|
|
gauche = analogRead(GAUCHE);
|
|
vitesseDroit(180);
|
|
vitesseGauche(-180);
|
|
}
|
|
}
|
|
|