Piloter un robot dans un labyrinthe sans capteur distance ultrason.

#include <Servo.h>                           // Include servo library
Servo servoLeft;                             // Declare left and right servos
Servo servoRight;

byte wLeftOld;                               // Previous loop whisker values
byte wRightOld;                              
byte counter;                                // For counting alternate corners
int count; // compte le nbre de clignotement de la LED rouge

unsigned long  currentTime =0;
unsigned long  previousTime =0;
bool ledstate=LOW;


void setup()                                 // Built-in initialization block
{ 
  Serial.begin(9600);
  pinMode(3, INPUT);                         // Set right whisker pin to input
  pinMode(2, INPUT);                         // Set left whisker pin to input  
  pinMode(13,OUTPUT);
  
/*
  tone(4, 3000, 1000);                       // Play tone for 1 second
  delay(1000);                               // Delay to finish tone
*/

  servoLeft.attach(10);                      // Attach left signal to pin 13 
  servoRight.attach(9);                     // Attach right signal to pin 12

  wLeftOld = 0;                              // Init. previous whisker states
  wRightOld = 1;  
  counter = 0;  
  count=0;
}  
 
void loop()                                  // Main loop auto-repeats
{

  byte wLeft = digitalRead(2);               // Copy left result to wLeft  
  byte wRight = digitalRead(3);              // Copy right result to wRight
  


 //---------------------------COMPTEUR AVEC LA LED----------------------------------------

 currentTime = millis();
 if((currentTime - previousTime)>1000) {
  previousTime=currentTime;
  ledstate=!ledstate;
  digitalWrite(13,!ledstate);
  count++;
 }

  
  // Whisker Navigation

  if((wLeft == 0) && (wRight == 0))          // If both whiskers contact
  {
    backward(1000);                          // Back up 1 second
    turnRight(800); // Turn left about 120 degrees
    turnRight(800);
    count=count-2;
  }
  else if(wLeft == 0)                        // If only left whisker contact
  {
    backward(1000);                          // Back up 1 second
    turnRight(200);                          // Turn right about 60 degrees
    count=count-2;
  }
  else if(wRight == 0)                       // If only right whisker contact
  {
    backward(1000);                          // Back up 1 second
    turnLeft(200);                           // Turn left about 60 degrees
    count=count-2;
  }
  else if(count==18){
    turnLeft(500);
  }

  else if (count==26){
    turnLeft(500);
  }
  else if(count==32){
    turnRight(500);
    delay(1000);
    forward(1000);
    delay(1000);
    turnRight(800);
    turnLeft(1600);
    detach();
  }
  else                                       // Otherwise, no whisker contact
  {
    forward(20);                             // Forward 1/50 of a second   
  }

}

void forward(int time)                       // Forward function
{
  servoLeft.writeMicroseconds(1000);         // Left wheel counterclockwise
  servoRight.writeMicroseconds(2000);        // Right wheel clockwise
  delay(time);                 // Maneuver for time ms
    
    
}

void turnLeft(int time)                      // Left turn function
{
  servoLeft.writeMicroseconds(1000);         // Left wheel clockwise
  servoRight.writeMicroseconds(1000);        // Right wheel clockwise
  delay(time);                               // Maneuver for time ms
}

void turnRight(int time)                     // Right turn function
{
  servoLeft.writeMicroseconds(2000);         // Left wheel counterclockwise
  servoRight.writeMicroseconds(2000);        // Right wheel counterclockwise
  delay(time);                               // Maneuver for time ms
  
}

void backward(int time)                      // Backward function
{
  servoLeft.writeMicroseconds(2000);         // Left wheel clockwise
  servoRight.writeMicroseconds(1000);        // Right wheel counterclockwise
  delay(time);                               // Maneuver for time ms
}

void stop()                      // Backward function
{
  servoLeft.writeMicroseconds(1500);         // Left wheel clockwise
  servoRight.writeMicroseconds(1500);        // Right wheel counterclockwise
  delay(5000);                               // Maneuver for time ms
}

void detach(){
    servoLeft.detach();                      
    servoRight.detach();                    
}

void attach(){
  servoLeft.attach(10);                    
  servoRight.attach(9);                
}