Robot de seguimiento de línea basado en inteligencia artificial (AI)

Código

• RoBot_Maze_Solve_2
• Funciones
• Funciones generales.
• sensorFuntions
• RobotDefines.h
• MotorFuntions

RoBot_Maze_Solve_2 C / C ++

 / * --------------------------------------------- --------------------- 7 sensores Smart Robot - Maze Solver y Line Follower con controlador PID programable a través de BT ==> Movimiento básico basado en Nano Mouse Robot, desarrollado por Michael Backus (http://www.akrobotnerd.com/) ==> Seguimiento de línea basado en http://samvrit.tk/tutorials/pid-control-arduino-line-follower-robot/?ckattempt=1==> CRÉDITO a Patrick McCabe por el código de resolución de ruta, visite patrickmccabemakes.com !! Marcelo Jose Rovai - 23 de abril de 2016 - Visite:http://mjrobot.org---------------- -------------------------------------------------- - * / # incluye  #include "robotDefines.h" Comando de cadena; Dispositivo de cadena; // Módulo BT # incluye  SoftwareSerial BT1 (10, 11); // El pin 10 es Rx y el pin 11 es Tx // ----------------------------------- ---------- configuración vacía () {Serial.begin (9600); BT1.begin (9600); pinMode (ledPin, SALIDA); pinMode (buttonPin, INPUT_PULLUP); // sensores de seguimiento de línea pinMode (lineFollowSensor0, INPUT); pinMode (lineFollowSensor1, ENTRADA); pinMode (lineFollowSensor2, ENTRADA); pinMode (lineFollowSensor3, ENTRADA); pinMode (lineFollowSensor4, ENTRADA); // servos leftServo.attach (5); rightServo.attach (3); BT1.print ("comprobar las constantes PID que se enviarán al robot"); Serial.print ("comprobar las constantes PID que se enviarán al robot"); BT1.println ('\ n'); while (digitalRead (buttonPin) &&! mode) {checkBTcmd (); // verifica si se recibe un comando del control remoto BT manualCmd (); comando =""; } checkPIDvalues ​​(); modo =DETENIDO; estado =0; // 1er paso} void loop () {ledBlink (1); BT1.println ("Iniciar primera pasada"); Serial.println ("Iniciar primera pasada"); readLFSsensors (); mazeSolve (); // Primera pasada para resolver el laberinto ledBlink (2); BT1.println ("Finalizar primera pasada"); Serial.println ("Finalizar primera pasada"); while (digitalRead (buttonPin) &&! mode) {checkBTcmd (); // verifica si se recibe un comando del control remoto BT manualCmd (); comando =""; } BT1.println ("Iniciando la segunda pasada"); Serial.println ("Iniciando la segunda pasada"); pathIndex =0; estado =0; mazeOptimization (); // ejecuta el laberinto lo más rápido posible ledBlink (3); BT1.println ("Finalizar la segunda pasada"); Serial.println ("Finalizar segunda pasada"); while (digitalRead (buttonPin) &&! mode) {checkBTcmd (); // verifica si se recibe un comando del control remoto BT manualCmd (); comando =""; } modo =DETENIDO; estado =0; // 1er paso pathIndex =0; pathLength =0;} 

Funciones C / C ++

 void mazeSolve (void) {while (! status) {readLFSsensors (); cambiar (modo) {caso NO_LINE:motorStop (); goAndTurn (IZQUIERDA, 180); recIntersection ('B'); descanso; caso CONT_LINE:runExtraInch (); readLFSsensors (); if (modo! =CONT_LINE) {goAndTurn (IZQUIERDA, 90); recIntersection ('L');} // o es una "T" o una "Cruz"). En ambos casos, va a la IZQUIERDA else mazeEnd (); descanso; case RIGHT_TURN:runExtraInch (); readLFSsensors (); if (modo ==NO_LINE) {goAndTurn (DERECHA, 90); recIntersection ('R');} else recIntersection ('S'); descanso; case LEFT_TURN:goAndTurn (IZQUIERDA, 90); recIntersection ('L'); descanso; case FOLLOWING_LINE:nextLine (); descanso; }}} // --------------------------------------------- recIntersection vacío (dirección de caracteres) {ruta [longitud de ruta] =dirección; // Almacena la intersección en la variable de ruta. pathLength ++; simplifyPath (); // Simplifica la ruta aprendida.} // ---------------------------------------- ------ void mazeEnd (vacío) {motorStop (); BT1.print ("El final ==> Ruta:"); for (int i =0; i "); Serial.println (pathLength); estado =1; mode =STOPPED;} // ------------------------------------------- --- void nextLine (void) {// readLFSsensors (); calcularPID (); motorPIDcontrol (); } // ----------------------------------------------- ------------------------------------------- // Simplificación de ruta. La estrategia es que cada vez que nos encontramos con una secuencia // xBx, podemos simplificarla eliminando el callejón sin salida. Por // ejemplo, LBL -> S, porque una sola S pasa por alto el callejón sin salida // representado por LBL.void simplifyPath () {// solo simplifica la ruta si el penúltimo turno fue una 'B' si ( pathLength <3 || path [pathLength-2]! ='B') return; int totalAngle =0; int i; for (i =1; i <=3; i ++) {switch (path [pathLength-i]) {case 'R':totalAngle + =90; descanso; caso 'L':totalAngle + =270; descanso; caso 'B':totalAngle + =180; descanso; }} // Obtenga el ángulo como un número entre 0 y 360 grados. totalAngle =totalAngle% 360; // Reemplaza todos esos giros con uno solo. switch (totalAngle) {caso 0:ruta [rutaLongitud - 3] ='S'; descanso; caso 90:ruta [pathLength - 3] ='R'; descanso; caso 180:ruta [pathLength - 3] ='B'; descanso; caso 270:ruta [pathLength - 3] ='L'; descanso; } // La ruta ahora es dos pasos más corta. pathLength - =2; } // ----------------------------------------------- ----------------------------------------------- void mazeOptimization ( void) {while (! status) {readLFSsensors (); cambiar (modo) {caso FOLLOWING_LINE:nextLine (); descanso; caso CONT_LINE:if (pathIndex> =pathLength) mazeEnd (); else {mazeTurn (ruta [índice de ruta]); pathIndex ++;} descanso; case LEFT_TURN:if (pathIndex> =pathLength) mazeEnd (); else {mazeTurn (ruta [índice de ruta]); pathIndex ++;} descanso; case RIGHT_TURN:if (pathIndex> =pathLength) mazeEnd (); else {mazeTurn (ruta [índice de ruta]); pathIndex ++;} descanso; }}} // --------------------------------------------- -------- void mazeTurn (char dir) {switch (dir) {case 'L':// Gire a la izquierda goAndTurn (IZQUIERDA, 90); descanso; case 'R':// Gire a la derecha goAndTurn (DERECHA, 90); descanso; case 'B':// Dar marcha atrás goAndTurn (DERECHA, 800); descanso; case 'S':// Ir directamente runExtraInch (); descanso; }} 

Funciones generales. C / C ++

 void ledBlink (int times) {for (int i =0; i  0) {Serial.print ("Comando recibido de BT ==>"); Serial.println (dispositivo); comando =dispositivo; dispositivo =""; // Resetea la variable BT1.flush (); }} // ---------------------------------------------- -------------------------- void manualCmd () {switch (comando [0]) {case 'g':mode =FOLLOWING_LINE; descanso; caso 's':motorStop (); // apaga ambos motores break; caso 'f':motorForward (); descanso; caso 'r':motorTurn (DERECHA, 30); motorStop (); descanso; caso 'l':motorTurn (IZQUIERDA, 30); motorStop (); descanso; caso 'b':motorBackward (); descanso; caso 'p':Kp =comando [2]; descanso; caso 'i':Ki =comando [2]; descanso; caso 'd':Kd =comando [2]; descanso; }} // ---------------------------------------------- -------------------------- void sendBTdata (int data) // envía datos a BT {digitalWrite (ledPin, HIGH); BT1.print ("Datos de Arduino"); BT1.print (datos); BT1.print ("xx"); BT1.println ('\ n'); digitalWrite (ledPin, BAJO);} // ---------------------------------------- ---------------- void calculatePID () {P =error; I =I + error; D =error-previousError; PIDvalue =(Kp * P) + (Ki * I) + (Kd * D); previousError =error;} // ------------------------------------------- ------------- void checkPIDvalues ​​() {BT1.print ("PID:"); BT1.print (Kp); BT1.print ("-"); BT1.print (Ki); BT1.print ("-"); BT1.println (Kd); Serial.print ("PID:"); Serial.print (Kp); Serial.print ("-"); Serial.print (Ki); Serial.print ("-"); Serial.println (Kd); } // ----------------------------------------------- void testLineFollowSensors () {int LFS0 =digitalRead (lineFollowSensor0); int LFS1 =digitalRead (lineFollowSensor1); int LFS2 =digitalRead (lineFollowSensor2); int LFS3 =digitalRead (lineFollowSensor3); int LFS4 =digitalRead (lineFollowSensor4); Serial.print ("LFS:L 0 1 2 3 4 R ==>"); Serial.print (LFS0); Serial.print (""); Serial.print (LFS1); Serial.print (""); Serial.print (LFS2); Serial.print (""); Serial.print (LFS3); Serial.print (""); Serial.print (LFS4); Serial.print ("==>"); Serial.print ("P:"); Serial.print (P); Serial.print ("I:"); Serial.print (I); Serial.print ("D:"); Serial.print (D); Serial.print ("PID:"); Serial.println (PIDvalue);} 

sensorFuntions C / C ++

 // --------------------------------------------- ---------------- / * leer valores de sensores de línea Valor de error de matriz de sensores 0 0 0 0 1 4 0 0 0 1 1 3 0 0 0 1 0 2 0 0 1 1 0 1 0 0 1 0 0 0 0 1 1 0 0-1 0 1 0 0 0-2 1 1 0 0 0-3 1 0 0 0 0 -4 1 1 1 1 1 0 Robot encontró línea continua - pruebe si hay una intersección o un final of maze0 0 0 0 0 0 Robot no encontró línea:girar 180o * / void readLFSsensors () {LFSensor [0] =digitalRead (lineFollowSensor0); LFSensor [1] =digitalRead (lineFollowSensor1); LFSensor [2] =digitalRead (lineFollowSensor2); LFSensor [3] =digitalRead (lineFollowSensor3); LFSensor [4] =digitalRead (lineFollowSensor4); farRightSensor =analogRead (farRightSensorPin); FarLeftSensor =analogRead (farLeftSensorPin); if ((LFSensor [0] ==1) &&(LFSensor [1] ==1) &&(LFSensor [2] ==1) &&(LFSensor [3] ==1) &&(LFSensor [4] ==1 )) {modo =CONT_LINE; error =0;} else if ((LFSensor [0] ==0) &&(farRightSensor "); Serial.print (farRightSensor); Serial.print ("modo:"); Serial.print (modo); Serial.print ("error:"); Serial.println (error); } // ----------------------------------------------- ------------ void readLateralSensors (void) {} ​​

RobotDefines.h C / C ++

 int mode =0; # define STOPPED 0 # define FOLLOWING_LINE 1 # define NO_LINE 2 # define CONT_LINE 3 # define POS_LINE 4 # define RIGHT_TURN 5 # define LEFT_TURN 6const int power =250; const int iniMotorPower =250; const int adj =0; float adjTurn =8; int extraInch =200; int adjGoAndTurn =800; const int ledPin =13; const int buttonPin =9; // LFSensor más a la izquierda es "0" const int lineFollowSensor0 =12; const int lineFollowSensor1 =18; const int lineFollowSensor2 =17; const int lineFollowSensor3 =16; const int lineFollowSensor4 =19; const int farRightSensorPin =0; // Pin analógico A0const int farLeftSensorPin =1; // Pin analógico A0const int THRESHOLD =150; int farRightSensor =0; int farLeftSensor =0; int LFSensor [5] ={0, 0, 0, 0, 0}; // PID controllerfloat Kp =50; float Ki =0; float Kd =0; float error =0, P =0, I =0, D =0, PIDvalue =0; float previousError =0, previousI =0; #define RIGHT 1 # define LEFT -1Servo leftServo; Servo rightServo; // ------------------------------------------------ - // Definiciones y variables específicas de la Fase 2 del laberinto (optimización) unsigned char dir; // La variable de ruta almacenará la ruta que ha tomado el robot:// 'L' para la izquierda // 'R' para la derecha // 'S' para la recta (en línea recta a través de una intersección) // 'B' para la espalda (Cambio de sentido) char path [100] =""; unsigned char pathLength =0; // la longitud del pathint pathIndex =0; unsigned int status =0; // resolviendo =0; llegar al final =1 

MotorFuntions C / C ++

 void motorStop () {leftServo.writeMicroseconds (1500); rightServo.writeMicroseconds (1500); retraso (200);} // ------------------------------------------ --- void motorForward () {leftServo.writeMicroseconds (1500 - (potencia + adj)); rightServo.writeMicroseconds (1500 + potencia);} // -------------------------------------- ------- void motorBackward () {leftServo.writeMicroseconds (1500 + potencia); rightServo.writeMicroseconds (1500 - potencia);} // -------------------------------------- ------- void motorFwTime (tiempo int sin firmar) {motorForward (); tiempo de retardo); motorStop ();} // ------------------------------------------- --void motorBwTime (tiempo int sin firmar) {motorBackward (); tiempo de retardo); motorStop ();} // ------------------------------------------- ----- void motorTurn (dirección int, grados int) {leftServo.writeMicroseconds (1500 - (iniMotorPower + adj) * dirección); rightServo.writeMicroseconds (1500 - dirección iniMotorPower *); delay (round (adjTurn * grados + 20)); motorStop ();} // ------------------------------------------- -------- void motorPIDcontrol () {int leftMotorSpeed ​​=1500 - (iniMotorPower + adj) - PIDvalue; int rightMotorSpeed ​​=1500 + iniMotorPower - PIDvalue; // La velocidad del motor no debe exceder la restricción del valor máximo de PWM (leftMotorSpeed, 1000, 2000); restringir (rightMotorSpeed, 1000, 2000); leftServo.writeMicroseconds (leftMotorSpeed); rightServo.writeMicroseconds (rightMotorSpeed); //Serial.print (PIDvalue); //Serial.print ("==> Izquierda, Derecha:"); //Serial.print (leftMotorSpeed); //Serial.print (""); //Serial.println (rightMotorSpeed);} // -------------------------------------- ------------- void runExtraInch (void) {motorPIDcontrol (); retraso (pulgada extra); motorStop ();} // ------------------------------------------- -------- void goAndTurn (dirección int, grados int) {motorPIDcontrol (); retraso (adjGoAndTurn); motorTurn (dirección, grados);} 

Esquemas