Petit problème pour faire fonctionner un vehicule autonome...

International Français
11

Voici mon code modifié:

//Tests robot autonome sur base à chenilles

// Déclaration de la bibliotech servo
#include <Servo.h> 
 
//Déclaration des points de sortie
const int trigPin = 6;
const int echoPin = 5;
const int mgV=3;
const int mdV=11;
const int mgS=12;
const int mdS=13;
const int brkD=9;
const int brkG=8;
const int servoPin=7;


//Déclaration du servo
Servo cmdsonar;

// Déclaration des variables
long duration;
int distance;
int distance_a;
int distance_d;
int distance_g;
int distance_mini=50;
int distance_secu=30;

void setup() {
pinMode(trigPin, OUTPUT); // Parmettrage du triger en sortie
pinMode(echoPin, INPUT); // Parmettrage de l'echo en entrée

// Déclaration des sorties de commande des moteurs

pinMode(mgS,OUTPUT);
pinMode(mdS,OUTPUT);
pinMode(brkD,OUTPUT);
pinMode(brkG,OUTPUT);

// Initialisation de la position de base du servomoteur
cmdsonar.attach(servoPin);
cmdsonar.write(90);

}

void loop() {
      distance_a=sonar();
  // conditions d'oriantation
    if(distance_a > distance_mini){
     avant();
     delay(300);
   }else{
      Stop();
      get_Distance();
      if(distance_g > distance_mini){
        droite();
        delay(200);
        avant_lent();
      }else if(distance_d > distance_mini){
        gauche();
        delay(200);
        avant_lent();
      }else if (distance_a < distance_mini){
        arriere();
        delay(200);
      }else{
        Stop();
      }
   }
}

// Définition du sonar
int sonar(){
  // Mise à l'état bas de l'entrée triger
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Mise à l'état haut de l'entrée triger pour 10 us et passage à l'état bas
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Lecture de l'entrée echo 
  int temp = pulseIn(echoPin, HIGH);
  // Calcul de la distance en fonction du temps parcouru par le son
  distance= temp*0.0343/2;
  return distance;
}


//Déclaration des fonctions des moteurs avec leur vitesse
void avant_lent(){
    digitalWrite(mgS,HIGH); 
    digitalWrite(mdS,HIGH);
    digitalWrite(brkD,LOW);
    digitalWrite(brkD,LOW);
    analogWrite(mgV,100);
    analogWrite(mdV,100);
}
void avant(){
    digitalWrite(mgS,HIGH);
    digitalWrite(mdS,HIGH);
    digitalWrite(brkD,LOW);
    digitalWrite(brkD,LOW);
    analogWrite(mgV,200);
    analogWrite(mdV,200);
}
void arriere(){
    digitalWrite(mgS,LOW);
    digitalWrite(mdS,LOW);
    digitalWrite(brkD,LOW);
    digitalWrite(brkD,LOW);
    analogWrite(mgV,123);
    analogWrite(mdV,123);
}
void gauche(){
    digitalWrite(mgS,LOW);
    digitalWrite(mdS,HIGH);
    digitalWrite(brkD,LOW);
    digitalWrite(brkD,LOW);
    analogWrite(mgV,123);
    analogWrite(mdV,123);
}
void droite(){
    digitalWrite(mgS,HIGH);
    digitalWrite(mdS,LOW);
    digitalWrite(brkD,LOW);
    digitalWrite(brkD,LOW);
    analogWrite(mgV,123);
    analogWrite(mdV,123);
}
void Stop(){
    digitalWrite(mgS,LOW);
    digitalWrite(mdS,LOW);
    digitalWrite(brkD,HIGH);
    digitalWrite(brkD,HIGH);
    analogWrite(mgV,0);
    analogWrite(mdV,0);

}

//Détection de la distance

//meusure à droite
void get_Distance(){
  cmdsonar.write(0);
  delay(200);
  int temp_d1=sonar();
  cmdsonar.write(45);
  delay(200);
  int temp_d2=sonar();
  if(temp_d1<temp_d2){
    distance_d=temp_d1;
  }else{
    distance_d=temp_d2;
  }
 //meusure au centre
  cmdsonar.write(90);
  delay(200);
  distance_a=sonar();
  
 //meusure à gauche
  cmdsonar.write(135);
  delay(200);
  int temp_g1=sonar();
  cmdsonar.write(180);
  delay(200);
  int temp_g2=sonar();
  if(temp_g1<temp_g2){
    distance_g=temp_g1;
  }else{
    distance_g=temp_g2;
  }
  cmdsonar.write(90);
  
}