corrections constantes

principal_ultrason.ino

@@ -92,6 +92,15 @@ int avance(void){
   return false;
 }
 
+int avanceSansCorrect(void){
+  con.move(255,255);
+  if((dr != -1 && dl != -1) && (dr < 25 || dl < 25)){
+    milli = millis();
+    return true;
+  }
+  return false;
+}
+
 // etat 4 on attend un peu d'avoir depassé la porte
 int depassePorte(void){
   if(dl > 30 and dr > 30){
@@ -144,12 +153,12 @@ int avanceCouloir(void){
   }
   
   // si on detecte l'ouverture
-  if(dl > 60){
+  if(dl > 80){
     con.stop();
     return true;
   }
 
-  float wdist = 20;
+  float wdist = 5;
   double diff = (dr-dl)/wdist;
   diff = constrain(diff, -1, 1);
   float dval = 10; // valeur maximal de l'objectif de la dérivée
@@ -221,7 +230,9 @@ int fin(void){
 // nos proto de fonctions, aucun arg, retourn un int (si vrai on avance dans la machine à état, sinon on reste dans l'état actuel)
 typedef int (*funcState)();
 // Le tableau des fonctions représentant les états
-funcState state[]={repos, checkBW, orient, avance, depassePorte, tournePorte, avancePorte, avanceCouloir, fin};
+//funcState state[]={repos, checkBW, orient, avance, depassePorte, tournePorte, avancePorte, avanceCouloir, fin};
+
+funcState state[]={repos, avanceSansCorrect, depassePorte, tournePorte, avancePorte, avanceCouloir, fin};
 // Le compteur stockant l'état actuel
 int stateInd=0;
 
@@ -230,6 +241,8 @@ unsigned int dcTimer = 0;
 // valeur de dl et dr au dernier calcul de derivée
 double ldl, ldr;
 
+int stateLeds[]={A4,A5,A6};
+
 void setup() {
   Serial.begin(9600);
   Serial.println("yo");
@@ -241,7 +254,7 @@ void setup() {
   pinMode(PR_L, INPUT);
   pinMode(PR_R, INPUT);
 
-  for(int i=4;i<=6;i++) pinMode(i, OUTPUT);
+  for(int i=0;i<=3;i++) pinMode(stateLeds[i], OUTPUT);
   
   // On initialise le controlleur des moteurs (set les pinMode et met toutes le sorties à 0V)
   con.init();
@@ -283,7 +296,7 @@ void loop() {
   Serial.println(stateInd);
 
   for(int i=0; i<3; i++) 
-    digitalWrite(i+4, (stateInd>>i)&1);
+    digitalWrite(stateLeds[i], (stateInd>>i)&1);
   
   // Si l'état retourn vrai, on incrémente le compteur d'état (i.e on passe à l'état suivant)
   if(state[stateInd]()) stateInd++;