Code Arduino - pilotage de deux MCC à l'aide de deux capteurs à ultrasons

[clemenceprepa]

Bonjour,

Je suis en CPGE PT, et je dois rendre pour le 9 juin mon projet de TIPE.
J'ai décidé de travailler sur un bras robotisé pilotable normalement via une télécommande, seulement, moi je souhaite le piloter grâce à des capteurs à ultrasons. En effet, ce bras robotisé est censé savoir analyser et rendre compte de son environnement, et donc grâce à deux capteurs à ultrasons placés dessus il détectera un objet et devra le saisir.
Je suis donc en train de coder un programme Arduino depuis maintenant un bon bout de temps, mais l'échéance arrivant à grand pas et mon code n'avançant pas j'aurai besoin d'aide.
Je vous explique ce que je souhaiterai coder:
Je voudrais contrôler le bras suivant 2 axes. Un premier moteur se mettrait en marche si et seulement si la distance entre le capteur et l'objet est inférieure ou égale à une distance minimale; sinon il reste éteint. Par la suite, lorsque ce moteur est en marche, lorsque la distance entre l'objet et le second capteur est inférieure à une distance minimale 2, le premier moteur s'éteint, le second s'allume pendant seulement 1s, puis à l'issu de cette seconde, le moteur 2 s'éteint et le moteur se rallume mais avec un sens de rotation inversé (pour que le bras puisse remonté après avoir saisi l'objet).
Je ne maitrise pas bien Arduino et mes codes arrêtent de fonctionner du jour au lendemain.
Je vous joins ici mon code pour que vous puissiez vous rendre compte de ce que j'ai fait.

#define trigPin 6     //Trig 
#define trigPin2 4    //Trig 
#define echoPin 7      //Echo
#define echoPin2 5      //Echo
int M1 = 12;
int M2 = 13;
int E1 = 3;
int E2 = 11;
int mindist = 20;

void setup() {
  // put your setup code here, to run once:
 pinMode(M1,OUTPUT);
 pinMode(M2,OUTPUT);
 Serial.begin (9600);  
 pinMode(trigPin, OUTPUT);  //On défini Trig comme une sortie
 pinMode(echoPin, INPUT);   //On défini Echo comme une entrée
 pinMode(trigPin2, OUTPUT);  //On défini Trig comme une sortie
 pinMode(echoPin2, INPUT);   //On défini Echo comme une entrée
}

void loop() {
  // put your main code here, to run repeatedly:
long duree, distance;
  digitalWrite(trigPin, LOW);  
  delayMicroseconds(2); 
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); //Trig envois pendant 10ms 
  digitalWrite(trigPin, LOW);
long duree2, distance2;
  digitalWrite(trigPin2, LOW);  
  delayMicroseconds(2); 
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10); //Trig envois pendant 10ms 
  digitalWrite(trigPin2, LOW);
  
  // On calcul le temps pour l'aller retour du signal
  duree = pulseIn(echoPin, HIGH);
  distance = duree*340/(2*10000); 
  // On calcul le temps pour l'aller retour du signal
  duree2 = pulseIn(echoPin2, HIGH);
  distance2 = duree2*340/(2*10000);  
   
    
  //Pour le moniteur série
  if ( distance <= 0){  
    Serial.println("Hors de portee");
  }
  else {
    Serial.print(distance);
    Serial.print(" cm ");
    Serial.print(duree);
    Serial.println(" ms");
  }
  delay(100);
  
 if ( distance2 <= 0){  
    Serial.println("Hors de portee");
  }
  else {
    Serial.print(distance2);
    Serial.print(" cm ");
    Serial.print(duree2);
    Serial.println(" ms");
  }
  delay(100);
  

   
  if (distance < mindist) {  
     digitalWrite(M1,HIGH);
     analogWrite(E1,255);
  }     
else {   //sinon on éteind la led
    digitalWrite(M1,LOW);
    analogWrite(E1,0);
  }

 
  
if (distance2 < mindist) { 
     digitalWrite(M2,HIGH);
     analogWrite(E2,255);
     digitalWrite(M1,LOW);
     analogWrite(E1,0);

}

}

Merci d'avance pour votre aide précieuse.

Clémence
-----