Robot Pengikut Garis Berbasis Artificial Intelligence (AI)

Kode

• RoBot_Maze_Solve_2
• Fungsi
• Fungsi Umum.
• Fungsi sensor
• RobotDefines.h
• Fungsi Motor

RoBot_Maze_Solve_2C/C++

/*-------------------------------------------------------- ---------------------7 sensor Robot Cerdas - Pemecah Labirin dan Pengikut Garis dengan pengontrol PID yang dapat diprogram melalui BT==> Gerakan dasar berdasarkan Robot Mouse Nano, yang dikembangkan oleh Michael Backus (http://www.akrobotnerd.com/ )==> Ikuti baris berdasarkan http://samvrit.tk/tutorials/pid-control-arduino-line-follower-robot/?ckattempt=1==> KREDIT ke Patrick McCabe untuk kode Pemecahan jalur, kunjungi patrickmccabemakes.com!!Marcelo Jose Rovai - 23 April 2016 - Kunjungi:http://mjrobot.org---------------- -------------------------------------------------- -*/#include #include "robotDefines.h"Perintah string;Perangkat string;// Modul BT#include SoftwareSerial BT1(10, 11); // El pin 10 es Rx y el pin 11 es Tx//----------------------------------- ----------pengaturan batal() { Serial.begin(9600); BT1.mulai (9600); pinMode(ledPin, OUTPUT); pinMode(tombolPin, INPUT_PULLUP); // sensor garis mengikuti pinMode(lineFollowSensor0, INPUT); pinMode(lineFollowSensor1, INPUT); pinMode(lineFollowSensor2, INPUT); pinMode(lineFollowSensor3, INPUT); pinMode(lineFollowSensor4, INPUT); // servos leftServo.attach(5); kananServo.attach(3); BT1.print("periksa konstanta PID yang akan dikirim ke Robot"); Serial.print("periksa konstanta PID yang akan dikirim ke Robot"); BT1.println('\n'); while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verifikasi jika perintah diterima dari BT remote control manualCmd(); perintah =""; } periksanilaiPID(); modus =DIHENTIKAN; status =0; // pass pertama}void loop() { ledBlink(1); BT1.println("Mulai Pass Pertama"); Serial.println("Mulai Pass Pertama"); readLFSsensor(); memecahkan labirin(); // Lulus pertama untuk memecahkan labirin ledBlink(2); BT1.println("Akhiri Pass Pertama"); Serial.println("Akhiri Pass Pertama"); while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verifikasi jika perintah diterima dari BT remote control manualCmd(); perintah =""; } BT1.println("Mulai Pass ke-2"); Serial.println("Memulai Pass ke-2"); indeks jalur =0; status =0; labirinOptimisasi(); //jalankan labirin secepat mungkin ledBlink(3); BT1.println("Akhiri Pass ke-2"); Serial.println("Akhiri Pass ke-2"); while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verifikasi jika perintah diterima dari BT remote control manualCmd(); perintah =""; } modus =DIHENTIKAN; status =0; // indeks jalur lintasan pertama =0; pathLength =0;}

FungsiC/C++

void mazeSolve(void){ while (!status) { readLFSsensors(); sakelar (mode) { kasus NO_LINE:motorStop(); goAndTurn (KIRI, 180); recIntersection('B'); merusak; kasus CONT_LINE:runExtraInch(); readLFSsensor(); if (mode !=CONT_LINE) {goAndTurn (KIRI, 90); recIntersection('L');} // atau itu adalah "T" atau "Cross"). Dalam kedua kasus, pergi ke LEFT else mazeEnd(); merusak; kasus RIGHT_TURN:runExtraInch(); readLFSsensor(); if (mode ==NO_LINE) {goAndTurn (KANAN, 90); recIntersection('R');} else recIntersection('S'); merusak; kasus LEFT_TURN:goAndTurn (KIRI, 90); recIntersection('L'); merusak; kasus FOLLOWING_LINE:followingLine(); merusak; } }}//-------------------------------------------------------- void recIntersection(arah char){ path[pathLength] =arah; // Simpan persimpangan di variabel jalur. panjang jalur ++; menyederhanakanPath(); // Sederhanakan jalur yang dipelajari.}//---------------------------------------- ------void mazeEnd(void){ motorStop(); BT1.print("Akhir ==> Jalur:"); for(int i=0;i "); Serial.println(Panjang jalur); status =1; mode =DIHENTIKAN;}//------------------------------------------- ---void followingLine(void){ //readLFSsensors(); hitungPID(); motorPIDkontrol(); }//----------------------------------------------------------- -------------------------------------------// Penyederhanaan jalur. Strateginya adalah setiap kali kita menemukan a// urutan xBx, kita dapat menyederhanakannya dengan memotong jalan buntu. Untuk// contoh, LBL -> S, karena satu S melewati jalan buntu// diwakili oleh LBL.void simplePath(){ // hanya menyederhanakan jalan jika belokan kedua-ke-terakhir adalah 'B' if( pathLength <3 || path[pathLength-2] !='B') kembali; int totalSudut =0; di aku; for(i=1;i<=3;i++) { switch(path[pathLength-i]) { case 'R':totalAngle +=90; merusak; kasus 'L':totalAngle +=270; merusak; kasus 'B':totalAngle +=180; merusak; } } // Dapatkan sudut sebagai angka antara 0 dan 360 derajat. totalAngle =totalAngle % 360; // Ganti semua putaran itu dengan satu putaran. switch(totalAngle) { case 0:path[pathLength - 3] ='S'; merusak; kasus 90:path[pathLength - 3] ='R'; merusak; kasus 180:path[pathLength - 3] ='B'; merusak; kasus 270:path[pathLength - 3] ='L'; merusak; } // Jalur sekarang dua langkah lebih pendek. pathLength -=2; } //------------------------------------------------ -----------------------------------------------Void mazeOptimization ( void){ while (!status) { readLFSsensors(); switch (mode) { kasus FOLLOWING_LINE:followingLine(); merusak; case CONT_LINE:if (pathIndex>=pathLength) mazeEnd(); else {mazeTurn (path[pathIndex]); pathIndex++;} istirahat; case LEFT_TURN:if (pathIndex>=pathLength) mazeEnd(); else {mazeTurn (path[pathIndex]); pathIndex++;} istirahat; case RIGHT_TURN:if (pathIndex>=pathLength) mazeEnd(); else {mazeTurn (path[pathIndex]); pathIndex++;} istirahat; } } }//-------------------------------------------------------- --------void mazeTurn (char dir) { switch(dir) { case 'L':// Belok Kiri goAndTurn (KIRI, 90); merusak; case 'R':// Belok Kanan goAndTurn (KANAN, 90); merusak; case 'B':// Putar Kembali goAndTurn (KANAN, 800); merusak; case 'S':// Jalan Lurus runExtraInch(); merusak; }}

Fungsi Umum.C/C++

void ledBlink(int kali){ for (int i =0; i 0) { Serial.print("Perintah diterima dari BT ==> "); Serial.println(perangkat); perintah =perangkat; perangkat =""; //Reset variabel BT1.flush(); } }//------------------------------------------------------------ --------------------------void manualCmd(){ switch (command[0]) { case 'g':mode =FOLLOWING_LINE; merusak; case 's':motorStop(); //matikan kedua motor break; huruf 'f':motorForward(); merusak; case 'r':motorTurn(KANAN, 30); berhenti motor(); merusak; case 'l':motorTurn(KIRI, 30); berhenti motor(); merusak; kasus 'b':motorBackward(); merusak; kasus 'p':Kp =perintah [2]; merusak; kasus 'i':Ki =perintah [2]; merusak; kasus 'd':Kd =perintah [2]; merusak; }}//--------------------------------------------- --------------------------void sendBTdata (int data) // kirim data ke BT{ digitalWrite (ledPin, HIGH); BT1.print("Data dari Arduino"); BT1.cetak(data); BT1.print("xx"); BT1.println('\n'); digitalWrite (ledPin, LOW);}//--------------------------------------------------- ----------------void calPID(){ P =error; saya =saya + kesalahan; D =error-previousError; Nilai PID =(Kp*P) + (Ki*I) + (Kd*D); sebelumnyaError =kesalahan;}//------------------------------------------------------- -------------void checkPIDvalues(){ BT1.print("PID:"); BT1.cetak(Kp); BT1.print(" - "); BT1.cetak(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("Saya:"); Serial.print (I); Serial.print("D:"); Serial.print (D); Serial.print("PID:"); Serial.println (nilai PID);}

sensorFuntionsC/C++

//--------------------------------------------- ------------------* membaca nilai sensor garis Nilai Kesalahan Array Sensor0 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 menemukan garis kontinu - uji apakah persimpangan atau ujung dari maze0 0 0 0 0 0 Robot tidak menemukan garis:putar 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 )&&(LFSensor[3]==1 )&&(LFSensor[4]==1 )) {mode =CONT_LINE; error =0;} else if((LFSensor[0]==0 )&&(farRightSensor "); Serial.print (farRightSensor); Serial.print("modus:"); Serial.print (modus); Serial.print("kesalahan:"); Serial.println (kesalahan); }//----------------------------------------------------------- ------------void readLateralSensors(void){ }

RobotDefines.hC/C++

int mode =0;# tentukan STOPPED 0# tentukan FOLLOWING_LINE 1# tentukan NO_LINE 2# tentukan CONT_LINE 3# tentukan POS_LINE 4# tentukan RIGHT_TURN 5# tentukan 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 lebih ke kiri adalah "0"const int lineFollowSensor0 =12; const int barisFollowSensor1 =18; const int barisFollowSensor2 =17; const int lineFollowSensor3 =16;const int lineFollowSensor4 =19;const int farRightSensorPin =0; //Pin Analog A0const int farLeftSensorPin =1; //Pin Analog 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, sebelumnyaI=0;#define RIGHT 1#define LEFT -1Servo leftServo;Servo rightServo; //------------------------------------------------ -//Maze Khusus Fase 2 (optimasi) definisi dan variabel unsigned char dir; // Variabel jalur akan menyimpan jalur yang telah diambil robot:// 'L' untuk kiri// 'R' untuk kanan// 'S' untuk lurus (lurus melalui persimpangan)// 'B' untuk belakang (putaran U) jalur karakter[100] =""; jalur karakter yang tidak ditandatanganiPanjang =0; // panjang pathIndex pathint =0;unsigned int status =0; // penyelesaian =0; mencapai akhir =1

MotorFuntionsC/C++

void motorStop(){ leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500); delay(200);}//------------------------------------------------------ --- void motorForward(){ leftServo.writeMicroseconds(1500 - (power+adj)); rightServo.writeMicroseconds(1500 + power);}//-------------------------------------- -------void motorBackward(){ leftServo.writeMicroseconds(1500 + power); rightServo.writeMicroseconds(1500 - power);}//-------------------------------------- -------void motorFwTime (waktu int tidak ditandatangani){ motorForward(); waktu penundaan); motorStop();}//------------------------------------------- --void motorBwTime (waktu int tidak ditandatangani){ motorBackward(); waktu penundaan); motorStop();}//------------------------------------------- -----void motorTurn(int arah, int derajat){ leftServo.writeMicroseconds(1500 - (iniMotorPower+adj)*arah); rightServo.writeMicroseconds(1500 - iniMotorPower*direction); delay (putaran(menyesuaikanTurn*derajat+20)); motorStop();}//------------------------------------------- --------void motorPIDcontrol(){ int leftMotorSpeed ​​=1500 - (iniMotorPower+adj) - nilai PID; int rightMotorSpeed ​​=1500 + iniMotorPower - nilai PID; // Kecepatan motor tidak boleh melebihi batasan nilai PWM maks(leftMotorSpeed, 1000, 2000); kendala (rightMotorSpeed, 1000, 2000); leftServo.writeMicroseconds(leftMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed); //Serial.print (nilai PID); //Serial.print(" ==> Kiri, Kanan:"); //Serial.print (kiriMotorSpeed); //Serial.print(" "); //Serial.println (rightMotorSpeed);}//-------------------------------------- -------------void runExtraInch(void){ motorPIDcontrol(); penundaan (ekstraInci); motorStop();}//------------------------------------------- --------void goAndTurn(int arah, int derajat){ motorPIDcontrol(); tunda(adjGoAndTurn); motorTurn(arah, derajat);}

Skema