Codage

De WIKI SN RASCOL
Aller à la navigation Aller à la recherche
  1. include <PWMServo.h>
  2. define IN1 7
  3. define IN2 8
  4. define ENA 5 // pin ENA
  1. define FRONT 90 //degré lorsque la direction est alignée droit devant

int SHARP_RIGHT=FRONT+33; int SHARP_LEFT=FRONT-40; int RIGHT=FRONT+17; int LEFT=FRONT-20;

  1. define SENSOR_FRONT 90

int SENSOR_LEFT=SENSOR_FRONT+25; int SENSOR_RIGHT=SENSOR_FRONT-25; int SENSOR_FAR_LEFT=SENSOR_FRONT+60; int SENSOR_FAR_RIGHT=SENSOR_FRONT-60;

  1. define DELAY_TIME 1000
  1. define SPEED 170
  2. define FAST_SPEED 200
  3. define MID_SPEED 170
  4. define SERVO_STEER 9 //Servo de direction connecté à D9
  5. define SERVO_SENSOR 10 //Capteur ultrasonore servo connecté à D10
  1. define Echo_PIN 2 // La broche Echo de l'ultrason est connectée à D11
  2. define Trig_PIN 3 // La broche Trig de l'ultrason est connectée à D12

PWMServo head; PWMServo head_steer; int leftscanval, centerscanval, rightscanval, ldiagonalscanval, rdiagonalscanval; const int distancelimit = 30; //Limite de distance pour les obstacles devant const int sidedistancelimit = 30; //Distance minimale en centimètres aux obstacles de chaque côté (la voiture permettra une distance latérale plus courte) int distance; int numcycles = 0; const int forwardtime = 600; //Temps que le robot passe à tourner (en millisecondes) const int turntime = 900; //Temps que le robot passe à tourner (en millisecondes) const int backtime = 900; //Temps passé par le robot à tourner (en millisecondes)

  1. define LPT 2 // Compteur de boucles de balayage.

int thereis; /*motor control*/ void go_Back(int speed) //Avancer { 

 digitalWrite(IN1, HIGH);
 digitalWrite(IN2,LOW);
 analogWrite(ENA,speed);

}

void go_Advance(int speed) //Avancer { 

 digitalWrite(IN1, LOW);
 digitalWrite(IN2,HIGH);
 analogWrite(ENA,speed);

}

void turn(int angle) { 

 head_steer.write(angle);

}

void stop_Stop() //Stop { 

 digitalWrite(IN1, LOW);
 digitalWrite(IN2,LOW);
 analogWrite(ENA,0);

}


void setup() { 

pinMode(ENA, OUTPUT); 
pinMode(IN1, OUTPUT); 
pinMode(IN2, OUTPUT); 
pinMode(Trig_PIN, OUTPUT); 
pinMode(Echo_PIN,INPUT); 

head_steer.attach(SERVO_STEER);
head.attach(SERVO_SENSOR); 
head.write(90);
turn(FRONT);
stop_Stop();
delay(2000);

Serial.begin(9600);

}


void loop() { 

auto_avoidance();

} /*detection of ultrasonic distance*/ int watch(){ 

 long echo_distance;
 digitalWrite(Trig_PIN,LOW);
 delayMicroseconds(5);                                                                              
 digitalWrite(Trig_PIN,HIGH);
 delayMicroseconds(15);
 digitalWrite(Trig_PIN,LOW);
 echo_distance=pulseIn(Echo_PIN,HIGH);
 echo_distance=echo_distance*0.01657; //À quelle distance se trouve l'objet en centimètres
 //Serial.println((int)echo_distance);
 return round(echo_distance);

} //Mesure les distances à droite, à gauche, devant, en diagonale à gauche, en diagonale à droite et assigne-les en centimètres aux variables rightscanval //leftscanval, centerscanval, ldiagonalscanval et rdiagonalscanval (il y a 5 points pour le test de distance) String watchsurrounding(){


int obstacle_status =B100000; 

 centerscanval = watch();
 if(centerscanval<distancelimit){
   stop_Stop();
   
   obstacle_status  =obstacle_status | B100;
   }
 head.write(SENSOR_LEFT);
 delay(100);
 ldiagonalscanval = watch();
 if(ldiagonalscanval<distancelimit){
   stop_Stop();
   
    obstacle_status  =obstacle_status | B1000;
   }
 head.write(SENSOR_FAR_LEFT); //Je n'ai pas utilisé 180 degrés car mon servo n'est pas capable de prendre cet angle
 delay(300);
 leftscanval = watch();
 if(leftscanval<sidedistancelimit){
   stop_Stop();
   
    obstacle_status  =obstacle_status | B10000;
   }

 head.write(SENSOR_FRONT); //Utilisez 90 degrés si vous faites tourner votre servo sur les 180 degrés complets.
 delay(100);
 centerscanval = watch();
 if(centerscanval<distancelimit){
   stop_Stop();
   
   obstacle_status  =obstacle_status | B100;
   }
 head.write(SENSOR_RIGHT);
 delay(100);
 rdiagonalscanval = watch();
 if(rdiagonalscanval<distancelimit){
   stop_Stop();
   
   obstacle_status  =obstacle_status | B10;
   }
 head.write(SENSOR_FAR_RIGHT);
 delay(100);
 rightscanval = watch();
 if(rightscanval<sidedistancelimit){
   stop_Stop();
 
   obstacle_status  =obstacle_status | 1;
   }
 head.write(SENSOR_FRONT); //Terminer de regarder autour (regarder à nouveau vers l'avant).
 delay(300);
  String obstacle_str= String(obstacle_status,BIN);
 obstacle_str= obstacle_str.substring(1,6);
 
 return obstacle_str; //Retourner une chaîne de 5 caractères représentant l'état des obstacles dans 5 directions

}

void auto_avoidance(){ 

 ++numcycles;
 if(numcycles>=LPT){ //Surveillez si quelque chose se trouve autour à chaque boucle LPT tout en avançant. 
    stop_Stop();
   String obstacle_sign=watchsurrounding(); // Les 5 chiffres de la valeur binaire obstacle_sign représentent l'état des obstacles dans les 5 directions
     Serial.print("begin str=");
       Serial.println(obstacle_sign);
                   if( obstacle_sign=="00000"){
    Serial.println("FORWARD");
    turn(FRONT);
    go_Advance(SPEED);

     delay(forwardtime);
     stop_Stop();
   }

   else if( obstacle_sign=="01000" || obstacle_sign=="11000" || obstacle_sign=="10000"  ){
    Serial.println("hand right");
          turn(SHARP_RIGHT);
    go_Advance(FAST_SPEED);
     delay(turntime);
     stop_Stop();
     turn(FRONT);
   } 
   
   else if( obstacle_sign=="00010" ||obstacle_sign=="00011" ||obstacle_sign=="00001"  ){
   Serial.println("hand left");
         turn(SHARP_LEFT);
    go_Advance(FAST_SPEED);
     delay(turntime);
     stop_Stop();
     turn(FRONT);
   }

   else if( obstacle_sign=="00111" ||   obstacle_sign=="01111" ||  obstacle_sign=="10111" || obstacle_sign=="11111"  || obstacle_sign=="00110" || obstacle_sign=="01010"  || obstacle_sign=="00101" ){
   Serial.println("hand back right");
    turn(RIGHT);
    go_Back(FAST_SPEED);
      delay(backtime);
         stop_Stop();
         turn(FRONT);
    } 
        else if(obstacle_sign=="00100"  || obstacle_sign=="10100"  || obstacle_sign=="01100" || obstacle_sign=="11100" || obstacle_sign=="11011"  ||    obstacle_sign=="11101"  ||  obstacle_sign=="11110"  || obstacle_sign=="01110"  ){
   Serial.println("hand back left");
     turn(LEFT);
    go_Back(FAST_SPEED);
      delay(backtime);
         stop_Stop();
         turn(FRONT);
    }    
 
     else Serial.println("no handle");
   numcycles=0; //Restart count of cycles
   } else {
            turn(FRONT);
    go_Advance(SPEED);
       delay(backtime);
         stop_Stop();
        
 }
 
 //else  Serial.println(numcycles);
 
 distance = watch(); // Utilisez la fonction watch() pour vérifier s'il y a quelque chose devant (lorsque le robot avance simplement sans regarder autour, il testera la distance devant).
 if (distance<distancelimit){ // Le robot s'arrêtera simplement s'il est complètement sûr qu'il y a un obstacle devant (il doit tester 25 fois) (nécessaire pour ignorer les faux signaux du capteur ultrasonique)
Serial.println("final go back");
      turn(SHARP_RIGHT);
    go_Back(FAST_SPEED);
 delay(backtime*3/2);
 turn(FRONT);
     ++thereis;}
 if (distance>distancelimit){
     thereis=0;} //Le compteur est réinitialisé
 if (thereis > 25){
 Serial.println("final stop");
   stop_Stop(); // Puisqu'il y a quelque chose devant, arrêtez de bouger.
   thereis=0;
 }

}

Cliquez ici pour retourner sur la page de la Voiture autonome mignature

Récupérée de « http://wikisn.rascol.net/index.php?title=Codage&oldid=993 »