PathFinder (11 / 14 étapes)

Etape 11 : Arduino - Code

Le code suivant a été édité en utilisant Arduino 1.6.7. Il s’agit de la liste complète du code.

 #include RedBotSensor left = RedBotSensor(A3); // initialize a left sensor object on A3 RedBotSensor center = RedBotSensor(A6); // initialize a center sensor object on A6 RedBotSensor right = RedBotSensor(A7); // initialize a right sensor object on A7// constants that are used in the code. LINETHRESHOLD is the level to detect // if the sensor is on the line or not. If the sensor value is greater than this // the sensor is above a DARK line. // // SPEED sets the nominal speed#define LINETHRESHOLD 800 #define SPEED 60 // sets the nominal speed. Set to any number from 0 - 255. #define CORRECTIONFACTOR 10 #define ADJUSTMENTTHRESHOLD 4000 //milliseconds to start an adjustment in advance RedBotMotors motors; int leftSpeed; // variable used to store the leftMotor speed int rightSpeed; // variable used to store the rightMotor speed unsigned long startTime; int correctionIndex = 0; int correctionCounter=0; unsigned long corrections[20]; int correctionsDir[20]; char action; bool ticket=false; void setup() { Serial.begin(9600); Serial.println("Welcome to experiment - Line Following"); Serial.println("------------------------------------------"); delay(2000); Serial.println("IR Sensor Readings: "); delay(500); startTime = millis(); corrections[0] = 0; leftSpeed=-SPEED; rightSpeed=SPEED; }void loop() {// Serial.print(left.read()); // Serial.print("\t"); // tab character // Serial.print(center.read()); // Serial.print("\t"); // tab character // Serial.print(right.read()); // Serial.println(); // Serial.print("Action: "); // tab character // Serial.print(action); // Serial.println(); if (action == 'g') { // if all sensors are on black or up in the air, stop the motors. // otherwise, run motors given the control speeds above. if ((left.read() > LINETHRESHOLD) || (center.read() > LINETHRESHOLD) ) { turn(0); } else if ( (center.read() > LINETHRESHOLD) || (right.read() > LINETHRESHOLD) ) { turn(1); } else { applyAdjustment(nextCorrection()); motors.leftMotor(leftSpeed); motors.rightMotor(rightSpeed); } delay(0); // add a delay to decrease sensitivity. } else { startTime = millis(); motors.brake(); } }void turn(int direct) { if (correctionCounter>=0)correctionCounter--; int adjust= correctionCounter*500; addCorrection(direct,adjust); motors.stop(); leftSpeed = 200; rightSpeed = -200; motors.leftMotor(leftSpeed); motors.rightMotor(rightSpeed); delay(100); if (direct == 0) { leftSpeed = -(SPEED - 50) * 2; } else { rightSpeed = SPEED - 50 * 2; } motors.leftMotor(leftSpeed); motors.rightMotor(rightSpeed); delay(250); leftSpeed = -SPEED; rightSpeed = SPEED; }void addCorrection(int Direction,int correctionTime) { corrections[correctionIndex] = millis() - startTime-correctionTime; correctionsDir[correctionIndex] = Direction; Serial.println("correction"); Serial.println(corrections[correctionIndex]); Serial.println(correctionsDir[correctionIndex]); correctionIndex++; correctionCounter++; }int nextCorrection() { int i = 0; unsigned long currentTime = millis() - startTime; while ((corrections[i] < currentTime) && (i < correctionIndex)) i++; return i; }void applyAdjustment( int nextCorrectionIndex) { unsigned long currentTime = millis() - startTime; unsigned long remainingTime = corrections[nextCorrectionIndex] - currentTime; //Time to correct if (remainingTime < ADJUSTMENTTHRESHOLD) { if (correctionsDir[nextCorrectionIndex] == 1) { rightSpeed = SPEED + applyWeight(remainingTime); leftSpeed = -(SPEED - applyWeight(remainingTime)); } else { rightSpeed = SPEED - applyWeight(remainingTime); leftSpeed = -(SPEED + applyWeight(remainingTime)); } ticket=true; } else if((currentTime > corrections[nextCorrectionIndex]) && (ticket==true)) { ticket=false; correctionIndex++; } else{ //No correction leftSpeed = -SPEED; rightSpeed = SPEED; } }int applyWeight(int wRemainingTime){ unsigned long currentTime = millis() - startTime; //int remainingTime=(int)abs(correctionsDir[nextCorrectionIndex]-currentTime); int newSpeed =0; if ((wRemainingTime=2000)){ newSpeed=CORRECTIONFACTOR*1; } else if ((wRemainingTime<2000)&(wRemainingTime>=1000)){ newSpeed=CORRECTIONFACTOR*2; } else if ((wRemainingTime<1000)&(wRemainingTime>=400)){ newSpeed=CORRECTIONFACTOR*3; } else if (wRemainingTime<400){ newSpeed=CORRECTIONFACTOR*1; } return newSpeed; }void serialEvent() { if (Serial.available()) { action = (char)Serial.read(); if (action == 'g') { correctionIndex=0; correctionCounter=0; } Serial.println("EVENTO"); } } 

Le code dans la méthode de « setup » est un code standard normal pour mettre en place la communication aux ports série et les informations de diagnostic. Les « retards » sont importants pour que vous donnez le temps de tout le système être en place et en cours d’exécution.

Le code de « boucle »

Le code de la boucle est en charge de la recherche en permanence à la valeur dans les capteurs. Quand un de ces capteurs atteint un seuil prédéfini le robot doit prendre les mesures appropriées.

Si le capteur de gauche atteint la bande (seuil), puis, le robot doit se tourner vers la droite. De même, si le capteur droit atteint la bande (seuil), le robot doit se tourner vers la gauche.

Le robot utilise une base de connaissances où il enregistre le temps quand il s’est écrasé contre la ligne. Au cours de chaque série, le robot enregistre chaque accident à l’avenir s’exécute, il recherchera dans sa base de connaissances pour « look-ahead » accidents avec et corrige sa trajectoire.

Il y a aussi des « ajustements ». Par exemple, disons que dans la première manche, le robot s’est écrasé à secondes 4 et 9. Dans l’accident à 4 seconde le robot devait revenir un peu pour corriger le chemin d’accès et donc perd quelque temps. Cela signifie que l’écrasement à 9 deuxième passerait à un autre moment dans les prochaines courses si l’accident à 4 seconde est évitée. Cette instance « abstention » est suivie dans la variable "correctionCounter". La direction de la correction est également suivie.

La méthode "applyWeight" est utilisée pour ajouter un poids à la vitesse de correction selon quelle distance nous sommes pour le prochain crash prédit. Le plus proche de l’accident suivant plus vite l’ajustement doit se faire.

Le code « tour »

Ce code rend le robot se tournent vers la gauche ou la droite selon les arguments passés à elle. Le travail réel de ce code doit être en mesure de déterminer la distance et la vitesse de se déplacer afin de se remettre sur la bonne voie. C’est fondamentalement déterminée par approche try et-en cas d’échec.

Voir comment la vitesse du moteur est initialement définie pour une base de 200 et -200 pour les moteurs de gauche et à droite respectivement. Ces valeurs de la vitesse fera le robot pour revenir en arrière.

Selon la direction de tourner, la valeur de la vitesse est recalculée pour un moteur ou d’une autre. Il s’agit de la façon dont nous prenons le robot pour revenir sur la bonne voie.

Aussi simple que ce code pourrait ressembler, ce qui rend le robot pour tout le monde tout type de chemin d’accès et lorsque vous arrivez à une fin, il peut trouver son chemin autour de revenir en arrière et recommencer le chemin d’accès.