Bonjour , je souhaiterais coder un robot afin qu'il puisse avancer tout en évitant des obstacles .
Sur notre robot nous disposons de 3 capteurs Sharp : un devant , et deux latéraux.
Nous avons déjà essayer de le coder mais il y a quelques défauts ;
-il évite les obstacles mais ses mouvements sont très brusques
-quand il y deux capteurs ou 3 qui sont actifs nous voulions qu'ils fassent marche arrière car sinon le robot ne répond plus .
Merci d'avance pour votre aide .
Voici notre code ;
//=====Déclaration des broches d'entrée=====================
const int Capteurs_SW = 0; //Entrée des 4 switchs en face avant
const int Capt_Sol = A1; // Capteur sol sous le robot
const int Capteurs_IR1 = A2 ; //Entrée analogique capteur SHARP Gauche
const int Capteurs_IR2 = A3; //Entrée analogique capteur SHARP Centre
const int Capteurs_IR3 = A4; //Entrée analogique capteur SHARP Droit
const int ledpin = 13; //Led sur la carte YUN
const int PWMG = 10; // Signal PWM Gauche vers le pont en H
const int PWMD = 9 ; // Signal PWM Droit vers le pont en H
const int SENSG = 12; // Signal SENS Gauche vers le pont en H
const int SENSD = 11; // Signal SENS Droit vers le pont en H
const int jack = 8; // Entrer du Jack de démarrage du robot
unsigned char state; //Détermine l'état de la machine à états
#define State_1 1
#define State_2 2
#define State_3 3
#define State_4 4
#define State_5 5
#define State_6 6
#define State_7 7
//=========================== Initialisation =========================
void setup()
{
pinMode (ledpin, OUTPUT);
pinMode (Capteurs_SW , INPUT );
pinMode (Capteurs_IR1, INPUT);
pinMode (Capteurs_IR2 , INPUT );
pinMode (Capteurs_IR3 , INPUT );
pinMode (jack , INPUT );
pinMode (SENSG, OUTPUT);
pinMode (PWMG , OUTPUT );
pinMode (SENSD, OUTPUT);
pinMode (PWMD , OUTPUT);
state = State_1;
}
//================= programme principal ===================
void loop()
{
switch(state) // Evolution de la machine à états en fonction des transitions
{
case State_1: { if (Jack_retire()==LOW) state=State_4;else state=State_2; break; }
case State_2: { if (Detect_obstacle_Devant()==HIGH) state=State_3;
if(Detect_obstacle_droit()==HIGH) state=State_5;
if(Detect_obstacle_droit()==HIGH and Detect_obstacle_Devant()==HIGH) state=State_7;
if (Detect_obstacle_gauche()==HIGH and Detect_obstacle_Devant()==HIGH) state=State_7;
if (Detect_obstacle_gauche()==HIGH and Detect_obstacle_Devant()==HIGH and Detect_obstacle_droit()==HIGH) state=State_7;
if (Detect_obstacle_gauche()==HIGH) state=State_6;
if(Detect_obstacle_droit()==HIGH) if (Detect_obstacle_gauche()==HIGH) state= State_7;break; }
case State_3: { if ( analogRead(Capteurs_IR2)>50 )state=State_7;else state=State_4 ; break; }
case State_5: { if ( analogRead(Capteurs_IR1)>50 )state=State_3;else state=State_4 ; break; }
case State_6: { if ( analogRead(Capteurs_IR3)>50 )state=State_3;else state=State_4 ; break; }
case State_4: { ( state=State_1); break; }
case State_7: { if ( analogRead(Capteurs_IR2)>200 ) state=State_7;else if ( analogRead(Capteurs_IR2)<200) state=State_4 ; break; }
}
switch(state) // Commande des actions en fonction des états
{
case State_1: { Arret_Moteur(); break; } // arrêt des 2 moteurs
case State_2: { Robot_Avance(); break; } // Robot avance droit
case State_3: { Arret_Moteur(); break; } // arrêt des 2 moteurs
case State_5: { Robot_tourneD(); break; } // le moteur tourne à Droite
case State_6: { Robot_tourneG(); break; } // le moteur tourne à Gauche
case State_7: { Robot_Recule(); break; } // Robot recule droit
}
}
//================ Fonction Arret_Moteur (Action)=============
void Arret_Moteur(void)
{
analogWrite (PWMD , 0 );
analogWrite (PWMG , 0 );
}
//================ Fonction Robot_Anvance (Action)===============
void Robot_Avance(void)
{
analogWrite (PWMD , 93 );
analogWrite (PWMG , 90);
digitalWrite (SENSG, LOW) ; //Moteur Gauche en avant
digitalWrite (SENSD, HIGH) ; //Moteur Droit en avant
}
//================ Fonction Robot_Recule (Action)===============
void Robot_Recule(void)
{
analogWrite (PWMD , 100 );
analogWrite (PWMG , 90);
digitalWrite (SENSG, HIGH) ; //Moteur Gauche en avant
digitalWrite (SENSD, LOW) ; //Moteur Droit en avant
}
//================ Fonction Robot_tourneG (Action)===============
void Robot_tourneD(void)
{
analogWrite (PWMD , 100 );
analogWrite (PWMG , 70 );
digitalWrite (SENSG, HIGH) ; //Moteur Gauche en arrière
digitalWrite (SENSD, HIGH) ; //Moteur Droit en avant
}
//================ Fonction Robot_tourneD (Action)===============
void Robot_tourneG(void)
{
analogWrite (PWMD , 100 );
analogWrite (PWMG , 70 );
digitalWrite (SENSG, LOW) ; //Moteur Gauche en avant
digitalWrite (SENSD, LOW) ; //Moteur Droit en arrière
}
//==============Fonction Jack_Retiré (Transition)====================
unsigned char Jack_retire(void) //retourne 1 si le jack est retiré
{
if(digitalRead(jack)==HIGH)return 1;
else return 0;
}
//============= Fonction Detect_obstacle_Devant (Transition)============
unsigned char Detect_obstacle_Devant(void) //retourne 1 si le capteur avant détecte un obstacle
{
if ( analogRead(Capteurs_IR2)>40 )return 1;
else return 0;
}
//============= Fonction Detect_obstacle_droit (Transition)============
unsigned char Detect_obstacle_droit(void) //retourne 1 si le capteur avant détecte un obstacle
{
if ( analogRead(Capteurs_IR3)>10 )return 1;
else return 0;
}
//============= Fonction Detect_obstacle_gauche (Transition)============
unsigned char Detect_obstacle_gauche(void) //retourne 1 si le capteur avant détecte un obstacle
{
if ( analogRead(Capteurs_IR1)>10 )return 1;
else return 0;
}