Op kunstmatige intelligentie (AI) gebaseerde robot die de robot volgt

Code

• RoBot_Maze_Solve_2
• Functies
• Algemene Functies.
• sensorFuntions
• RobotDefines.h
• Motorfuncties

RoBot_Maze_Solve_2C/C++

/*--------------------------------------------- ---------------------7 sensoren Smart Robot - Maze Solver en Line Follower met programmeerbare PID-controller via BT==> Basisbeweging gebaseerd op Nano Mouse Robot, ontwikkeld door Michael Backus (http://www.akrobotnerd.com/ )==> Lijn volgen op basis van http://samvrit.tk/tutorials/pid-control-arduino-line-follower-robot/?ckattempt=1==> CREDIT aan Patrick McCabe voor het pad Solving Code, bezoek patrickmccabemakes.com!!Marcelo Jose Rovai - 23 april 2016 - Bezoek:http://mjrobot.org---------------- -------------------------------------------------- -*/#include #include "robotDefines.h"String commando;String device;// BT Module#include SoftwareSerial BT1(10, 11); // El pin 10 es Rx y el pin 11 es Tx//----------------------------------- ---------- ongeldige setup () { Serial.begin (9600); BT1.begin (9600); pinMode (ledPin, UITGANG); pinMode (knopPin, INPUT_PULLUP); // lijnvolgsensoren pinMode (lineFollowSensor0, INPUT); pinMode(lineFollowSensor1, INPUT); pinMode(lineFollowSensor2, INPUT); pinMode (lineFollowSensor3, INPUT); pinMode (lineFollowSensor4, INPUT); // servo's leftServo.attach (5); rightServo.attach (3); BT1.print("controleer de PID-constanten die naar Robot moeten worden verzonden"); Serial.print ("controleer de PID-constanten die naar Robot moeten worden verzonden"); BT1.println('\n'); while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // controleer of een commando is ontvangen van BT-afstandsbediening manualCmd (); commando =""; } checkPIDwaarden(); modus =GESTOPT; status =0; // 1e pas} ongeldige lus () { ledBlink (1); BT1.println("Start First Pass"); Serial.println("Start First Pass"); leesLFSsensoren(); doolhofOplossen(); // Eerste pas om het doolhof op te lossen ledBlink(2); BT1.println("Einde eerste doorgang"); Serial.println("Einde eerste doorgang"); while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // controleer of een commando is ontvangen van BT-afstandsbediening manualCmd (); commando =""; } BT1.println("Begin 2e doorgang"); Serial.println("Begin 2e doorgang"); padIndex =0; status =0; doolhofoptimalisatie(); // voer het doolhof zo snel mogelijk uit ledBlink (3); BT1.println("Einde 2e doorgang"); Serial.println("Einde 2e doorgang"); while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // controleer of een commando is ontvangen van BT-afstandsbediening manualCmd (); commando =""; } modus =GESTOPT; status =0; // 1e doorgang pathIndex =0; pathLength =0;}

FunctiesC/C++

void mazeSolve(void){ while (!status) { readLFSsensors(); schakelaar (modus) {geval NO_LINE:motorStop(); goAndTurn (LINKS, 180); recIntersection('B'); pauze; geval CONT_LINE:runExtraInch(); leesLFSsensoren(); if (modus!=CONT_LINE) {goAndTurn (LINKS, 90); recIntersection('L');} // of het is een "T" of "Cross"). Ga in beide gevallen naar LEFT else mazeEnd(); pauze; case RIGHT_TURN:runExtraInch(); leesLFSsensoren(); if (modus ==NO_LINE) {goAndTurn (RECHTS, 90); recIntersection('R');} else recIntersection('S'); pauze; case LEFT_TURN:goAndTurn (LEFT, 90); recIntersection('L'); pauze; case FOLLOWING_LINE:volgendeLine(); pauze; } }}//--------------------------------------------- void recIntersection(char richting){ path[pathLength] =richting; // Sla de kruising op in de padvariabele. padLengte ++; vereenvoudigPad(); // Vereenvoudig het geleerde pad.}//---------------------------------------- ------void mazeEnd(void){ motorStop(); BT1.print("Het einde ==> Pad:"); for(int i=0;i "); Serial.println(padlengte); toestand =1; modus =GESTOPT;}//------------------------------------------- ---void volgendeLine(void){ //readLFSsensors(); berekenPID(); motorPIDcontrol(); }//----------------------------------------------- -------------------------------------------// Padvereenvoudiging. De strategie is dat wanneer we een // reeks xBx tegenkomen, we het kunnen vereenvoudigen door de doodlopende weg te verwijderen. Voor// bijvoorbeeld LBL -> S, omdat een enkele S de doodlopende weg omzeilt// vertegenwoordigd door LBL.void simplePath(){ // vereenvoudig het pad alleen als de voorlaatste bocht een 'B' was if( pathLength <3 || path[pathLength-2] !='B') return; int totaalHoek =0; int ik; for(i=1;i<=3;i++) { switch(path[pathLength-i]) { case 'R':totalAngle +=90; pauze; geval 'L':totaalAngle +=270; pauze; geval 'B':totaalAngle +=180; pauze; } } // Verkrijg de hoek als een getal tussen 0 en 360 graden. totale hoek =totale hoek % 360; // Vervang al die beurten door een enkele. switch(totalAngle) { case 0:path[pathLength - 3] ='S'; pauze; geval 90:path[pathLength - 3] ='R'; pauze; geval 180:path[pathLength - 3] ='B'; pauze; geval 270:path[pathLength - 3] ='L'; pauze; } // Het pad is nu twee stappen korter. padLengte -=2; } //----------------------------------------------- -----------------------------------------------void mazeOptimization ( void){ while (!status) { readLFSsensors(); switch (modus) { case FOLLOWING_LINE:volgendeLine(); pauze; case CONT_LINE:if (pathIndex>=pathLength) mazeEnd (); else {mazeTurn (pad [padIndex]); pathIndex++;} pauze; case LEFT_TURN:if (pathIndex>=pathLength) mazeEnd (); else {mazeTurn (pad [padIndex]); pathIndex++;} pauze; case RIGHT_TURN:if (pathIndex>=pathLength) mazeEnd (); else {mazeTurn (pad [padIndex]); pathIndex++;} pauze; } } }//--------------------------------------------- --------void mazeTurn (char dir) { switch (dir) { case 'L':// Sla linksaf goAndTurn (LEFT, 90); pauze; case 'R':// Sla rechtsaf goAndTurn (RECHTS, 90); pauze; geval 'B':// Draai terug goAndTurn (RECHTS, 800); pauze; case 'S':// Ga rechtdoor runExtraInch(); pauze; }}

Algemene Functies.C/C++

void ledBlink(int times){ for (int i =0; i 0) { Serial.print("Opdracht ontvangen van BT ==> "); Seriële.println(apparaat); commando =apparaat; apparaat =""; // Reset de variabele BT1.flush(); } }//---------------------------------------------- --------------------------void manualCmd(){ switch (command [0]) { case 'g':mode =FOLLOWING_LINE; pauze; geval 's':motorStop(); // schakel beide motoren uit; geval 'f':motorForward(); pauze; geval 'r':motorTurn(RIGHT, 30); motorStop(); pauze; geval 'l':motorTurn(LEFT, 30); motorStop(); pauze; geval 'b':motorBackward(); pauze; geval 'p':Kp =commando[2]; pauze; geval 'i':Ki =commando[2]; pauze; geval 'd':Kd =commando[2]; pauze; }}//---------------------------------------------- -------------------------- void sendBTdata (int data) // stuur data naar BT {digitalWrite (ledPin, HIGH); BT1.print("Gegevens van Arduino"); BT1.print(gegevens); BT1.print(" xx"); BT1.println('\n'); digitalWrite (ledPin, LOW);}//---------------------------------------- ----------------ongeldig berekenenPID(){ P =fout; ik =ik + fout; D =fout-vorigeError; PID-waarde =(Kp*P) + (Ki*I) + (Kd*D); previousError =fout;}//------------------------------------------- ------------- ongeldig checkPIDvalues(){ BT1.print("PID:"); BT1.print(Kp); BT1.print(" - "); BT1.print(Ki); BT1.print(" - "); BT1.println(Kd); Serial.print("PID:"); Seriële.afdruk (Kp); Serieel.print(" - "); Seriële.afdruk (Ki); Serieel.print(" - "); Seriële.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 ==> "); Serieafdruk (LFS0); Serieafdruk (" "); Serieafdruk (LFS1); Serieafdruk (" "); Serieafdruk (LFS2); Serieafdruk (" "); Serieafdruk (LFS3); Serieafdruk (" "); Serieafdruk (LFS4); Serie.afdruk (" ==> "); Serieafdruk ("P:"); Serieafdruk (P); Serieafdruk (" I:"); Serieafdruk (I); Serieafdruk ("D:"); Serieafdruk (D); Serial.print (" PID:"); Serial.println (PID-waarde);}

sensorFuntionsC/C++

//--------------------------------------------- ----------------/* lees lijn sensoren waarden Sensor Array Error Waarde 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 gevonden doorlopende lijn - test of een kruising of einde is van doolhof0 0 0 0 0 0 Robot heeft geen regel gevonden:draai 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 =analoogRead (farLeftSensorPin); if ((LFSensor[0]==1 )&&(LFSensor[1]==1 )&&(LFSensor[2]==1 )&&(LFSensor[3]==1 )&&(LFSensor[4]==1 )) {modus =CONT_LINE; error =0;} else if((LFSensor[0]==0 )&&(farRightSensor  "); Serial.print (farRightSensor); Serial.print ("modus:"); Serieafdruk (modus); Serial.print ("fout:"); Serial.println (fout); }//----------------------------------------------- ------------void readLateralSensors(void){ }

RobotDefines.hC/C++

int mode =0;# definiëren GESTOPT 0# definiëren FOLLOWING_LINE 1# definiëren NO_LINE 2# definiëren CONT_LINE 3# definiëren POS_LINE 4# definiëren RIGHT_TURN 5# definiëren 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 meer naar links is "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; // Analoge pin A0const int farLeftSensorPin =1; // Analoge pin 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; //------------------------------------------------ -//Specifieke Maze Phase 2 (optimalisatie) definities en variabelen unsigned char dir; // De padvariabele slaat het pad op dat de robot heeft genomen:// 'L' voor links// 'R' voor rechts// 'S' voor rechtdoor (rechtdoor gaan door een kruising)// 'B' voor terug (U-bocht)char path[100] ="";unsigned char pathLength =0; // de lengte van de pathint pathIndex =0; unsigned int status =0; // oplossen =0; bereik einde =1

MotorFuntionsC/C++

void motorStop(){ leftServo.writeMicroseconds(1500); rightServo.writeMicroseconden (1500); vertraging (200);}//------------------------------------------ --- void motorForward(){ leftServo.writeMicroseconds(1500 - (power+adj)); rightServo.writeMicroseconden (1500 + vermogen);}//-------------------------------------- -------void motorBackward(){ leftServo.writeMicroseconds (1500 + vermogen); rightServo.writeMicroseconds (1500 - vermogen);}//-------------------------------------- -------void motorFwTime (unsigned int time){ motorForward(); vertragingstijd); motorStop();}//------------------------------------------- --void motorBwTime (unsigned int time){ motorBackward(); vertragingstijd); motorStop();}//------------------------------------------- -----void motorTurn (int richting, int graden) { leftServo.writeMicroseconds(1500 - (iniMotorPower+adj)* richting); rightServo.writeMicroseconden (1500 - iniMotorPower*richting); vertraging (ronde(adjTurn*graden+20)); motorStop();}//------------------------------------------- --------void motorPIDcontrol(){int leftMotorSpeed ​​=1500 - (iniMotorPower+adj) - PIDvalue; int rightMotorSpeed ​​=1500 + iniMotorPower - PID-waarde; // De motorsnelheid mag de maximale PWM-waardebeperking niet overschrijden (leftMotorSpeed, 1000, 2000); beperking (rightMotorSpeed, 1000, 2000); leftServo.writeMicroseconden(linksMotorSpeed); rightServo.writeMicroseconden(rightMotorSpeed); //Serial.print (PID-waarde); //Serial.print (" ==> Links, Rechts:"); //Serial.print (leftMotorSpeed); //Serial.print (" "); //Serial.println (rightMotorSpeed);}//------------------------------------- ------------- ongeldig runExtraInch(void){ motorPIDcontrol(); vertraging (extra inch); motorStop();}//------------------------------------------- --------void goAndTurn (int richting, int graden){ motorPIDcontrol(); vertraging (adjGoAndTurn); motorTurn(richting, graden);}

Schema's