Robot Pengikut Garis Berbasis AI dengan Arduino Nano

Komponen dan Perlengkapan

Arduino Nano	1
Seeed Wireless Bluetooth RF Transceiver Master Slave	1
Amazon Web Services IR Infrared Sensor Module Reflective Photoelectric Light	1
Sony Continuous Rotation Servo	1
Android 4.8 Volt Battery	1

Aplikasi dan Layanan Online

Arduino IDE

Tentang Proyek Ini

Setelah mengembangkan robot Pick & Place berbasis Arduino, ide berikutnya adalah mengembangkan robot pengikut garis berbasis kecerdasan buatan (AI). Membuat robot pengikut garis sederhana cukup baik untuk pemula, yang diperlukan hanyalah menambahkan sejumlah sensor inframerah alih-alih motor servo dalam proyek saya terbaru. “Ide adalah untuk menguji keterampilan saya di tingkat lanjut.”

Meskipun tugas ini sulit, riset online membantu saya banyak.

Mengapa diperlukan robot pengikut garis berbasis AI?

Ketika datang pada AI, ini adalah teknologi terkini yang memiliki potensi untuk mengubah lingkungan dunia dan kehidupan manusia yang efektif. Impian yang dapat terwujud dengan memberi kecerdasan pada robot kita untuk memahami masalah kehidupan nyata dan menyelesaikannya sesuai itu, impian itu hanya dapat terwujud melalui Kecerdasan Buatan, itulah sebabnya semua orang membicarakannya.

• Dapat digunakan di rangkaian perakitan/produksi.
• Bus penjemputan penumpang.

Meskipun robot prototipe saya sedikit cerdas, ini hanyalah awal bagi saya untuk mengarahkan proyek-proyek saya menuju AI. Oleh karena itu, di sini saya akan mencoba menjelaskan secara bertahap pembangunan robot pengikut garis menggunakan Kecerdasan Buatan dengan bahasa yang cukup sederhana. Karena keterbatasan sumber daya, saya tidak akan membangun robot, tetapi saya dapat menjelaskan dengan sangat baik bagaimana Anda dapat membangunnya dengan melakukan sejumlah riset. Saya akan mencoba memberikan kode yang akurat yang akan bekerja di Arduino IDE Anda, sehingga Anda dapat mengembangkan robot AI pertama Anda dengan cukup mudah.

Anda dapat mendukung saya di Patreon dari sini: https://bit.ly/31NfQ6A

“Anda dapat menemukan peralatan termurah di Amerika Serikat di deskripsi video ini, cukup klik pada video.”

Sekarang mari kita bicara tentang proyek “Robot Pengikut Garis Berbasis AI!”.

Penggunaan Sensor Inframerah

Proyek kami mencakup 7 sensor inframerah seperti yang ditunjukkan dalam diagram.

• 5 untuk Kontrol PID
• 1 untuk deteksi kiri
• satu lagi untuk deteksi kanan.

Peran 5 Sensor Kontrol PID: 5 sensor ini akan digunakan untuk menghasilkan output digital baik tinggi atau rendah (1, 0) masing-masing.

Sensor yang berpusat terkait garis hitam, hanya sensor itu yang akan menghasilkan TINGGI(1). Hasil yang sama mungkin dari sensor tersebut adalah:-

• 10000
• 11000
• 01000
• 01100
• 00100
• 00110
• 00010
• 00011
• 00001
• 00000
• 11111

Sekarang mari kita bicara tentang dua sensor lain untuk hasil kiri dan kanan yang mungkin adalah

Sensor kiri jauh: output analog tinggi atau rendah

Sensor kiri jauh: output analog tinggi atau rendah

Untuk menyimpan nilai 5 sensor, mari kita buat variabel array.

in LFSensor[5]={1,1,1,1,1};

Untuk menyimpan nilai sensor kiri dan kanan, kita akan menggunakan integer

int farleft_sensor=0;

Seperti yang kita ketahui, kita punya 5 sensor yang dapat digunakan untuk menyimpan jalur kiri dan kanan yang diambil oleh robot dalam array. Jadi

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);

ATURAN TANGAN KIRI LABIRIN:

Tonton video ini untuk memahami aturan tangan kiri labirin

Singkatnya, Aturan Tangan Kiri dapat digambarkan sebagai:

• Letakkan tangan kiri Anda pada dinding.
• Mulai berjalan ke depan
• Di setiap persimpangan, dan di seluruh labirin, jaga tangan kiri Anda menyentuh dinding di sebelah kiri.
• Akhirnya, Anda akan mencapai akhir labirin. Anda mungkin tidak akan melintasi jalan tercepat dan paling langsung, tetapi Anda akan sampai di sana.

Jadi, kunci di sini adalah mengidentifikasi persimpangan, menentukan jalur mana yang akan diambil berdasarkan aturan di atas. Secara khusus, dalam jenis labirin 2D kami, kita dapat menemukan 8 jenis persimpangan yang berbeda (lihat gambar pertama di atas):

Melihat gambar, kita dapat menyadari bahwa tindakan yang mungkin di persimpangan adalah:

Di “Cross”

• Pergi ke Kiri, atau
• Pergi ke Kanan, atau
• Pergi Lurus
• Di “Cross” Pergi ke Kiri, atau Pergi ke Kanan, atau Pergi Lurus

Di “T”:

• Pergi ke Kiri, atau
• Pergi ke Kanan
• Di “T”: Pergi ke Kiri, atau Pergi ke Kanan

Di “Right Only”:

• Pergi ke Kanan
• Di “Right Only”: Pergi ke Kanan

Di “Left Only”:

• Pergi ke Kiri
• Di “Left Only”: Pergi ke Kiri

Di “Straight or Left”:

• Pergi ke Kiri, atau
• Pergi Lurus
• Di “Straight or Left”: Pergi ke Kiri, atau Pergi Lurus

Di “Straight or Right”:

• Pergi ke Kanan, atau
• Pergi Lurus
• Di “Straight or Right”: Pergi ke Kanan, atau Pergi Lurus

Di “Dead End”:

• Kembali ( “U turn”)
• Di “Dead End”: Kembali ( “U turn”)

Di “End of Maze”:

• Berhenti
• Di “End of Maze”: Berhenti

Tetapi, menerapkan “Aturan Tangan Kiri”, tindakan akan dikurangi menjadi satu opsi masing-masing:

• Di “Cross”: Pergi ke Kiri
• Di “T”: Pergi ke Kiri
• Di “Right Only”: Pergi ke Kanan
• Di “Left Only”: Pergi ke Kiri
• Di “Straight or Left”: Pergi ke Kiri
• Di “Straight or Right”: Pergi Lurus
• Di “Dead End”: Kembali ( “U turn”)
• Di “End of Maze”: Berhenti

Kami hampir sampai! “Tenang!”

Saat robot mencapai “Dead End” atau “End of a Maze”, mudah untuk mengidentifikasi mereka, karena mereka tidak memiliki situasi ambigu (kami telah mengimplementasikan tindakan tersebut di Robot Pengikut Garis, ingat?). Masalahnya adalah ketika robot menemukan “LINE” misalnya karena garis bisa menjadi “Cross” (1) atau “T” (2). Juga saat mencapai “LEFT or RIGHT TURN”, itu bisa menjadi belokan sederhana (opsi 3 atau 4) atau opsi untuk pergi lurus (5 atau 6). Untuk menemukan secara tepat jenis persimpangan apa yang dihadapi robot, langkah tambahan harus dilakukan: robot harus berlari “inci ekstra” dan melihat apa yang ada selanjutnya (lihat gambar kedua di atas, sebagai contoh).

Jadi, dalam hal alur, tindakan yang mungkin sekarang dapat digambarkan sebagai:

Di “DEAD END”:

• Kembali ( “U turn”)
• Di “DEAD END”: Kembali ( “U turn”)

Di “LINE”:

• Lari inci ekstra
• Jika ada garis: Itu adalah “Cross” ==> Pergi ke KIRI
• Jika tidak ada garis: itu adalah “T” ==> Pergi ke KIRI
• Jika ada garis lain: itu adalah “End of Maze” ==> BERHENTI

Di “RIGHT TURN”:

• Lari inci ekstra
• jika ada garis: Itu adalah Straight or Right ==> Pergi LURUS
• Jika tidak ada garis: itu adalah Right Only ==> Pergi ke KANAN

Di “LEFT TURN”:

• Lari inci ekstra
• jika ada garis: Itu adalah Straight or LEFT ==> Pergi ke KIRI
• Jika tidak ada garis: itu adalah LEFT Only ==> Pergi ke KIRI

Perhatikan bahwa sebenarnya, dalam kasus “LEFT TURN”, Anda dapat melewatkan pengujian, karena Anda akan mengambil KIRI bagaimanapun. Saya meninggalkan penjelasan lebih umum hanya untuk kejelasan. Dalam kode nyata, saya akan melewatkan pengujian ini.

Berikut adalah beberapa masalah yang mungkin timbul saat membuat proyek ini.

Topik Terkait

PENGATURAN DRIVER ARDUINO

Tonton video ini untuk menyelesaikan masalah driver Arduino :

Kode

• RoBot_Maze_Solve_2
• Functions
• GeneralFunctions.
• sensorFuntions
• RobotDefines.h
• MotorFuntions

RoBot_Maze_Solve_2C/C++

/*------------------------------------------------------------------ 7 sensors Smart Robot - Maze Solver and Line Follower with programable PID controller via BT ==> Basic movement based on Nano Mouse Robot, developed by Michael Backus (https://www.akrobotnerd.com/ ) ==> Line follow based on https://samvrit.tk/tutorials/pid-control-arduino-line-follower-robot/?ckattempt=1 ==> CREDIT to Patrick McCabe for the path Solving Code, visit patrickmccabemakes.com!! Marcelo Jose Rovai - 23 April, 2016 - Visit: https://mjrobot.org -------------------------------------------------------------------*/ #include <Servo.h> #include "robotDefines.h" String command; String device; // BT Module #include <SoftwareSerial.h> SoftwareSerial BT1(10, 11); // El pin 10 es Rx y el pin 11 es Tx //--------------------------------------------- void setup() { Serial.begin(9600); BT1.begin(9600); pinMode(ledPin, OUTPUT); pinMode(buttonPin, INPUT_PULLUP); // line follow sensors pinMode(lineFollowSensor0, INPUT); pinMode(lineFollowSensor1, INPUT); pinMode(lineFollowSensor2, INPUT); pinMode(lineFollowSensor3, INPUT); pinMode(lineFollowSensor4, INPUT); // servos leftServo.attach(5); rightServo.attach(3); BT1.print("check the PID constants to be sent to Robot"); Serial.print("check the PID constants to be sent to Robot"); BT1.println('\n'); while (digitalRead(buttonPin) && !mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command = ""; } checkPIDvalues(); mode = STOPPED; status = 0; // 1st pass } void loop() { ledBlink(1); BT1.println("Start First Pass"); Serial.println("Start First Pass"); readLFSsensors(); mazeSolve(); // First pass to solve the maze ledBlink(2); BT1.println("End First Pass"); Serial.println("End First Pass"); while (digitalRead(buttonPin) && !mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command = ""; } BT1.println("Starting 2nd Pass"); Serial.println("Starting 2nd Pass"); pathIndex = 0; status = 0; mazeOptimization(); //run the maze as fast as possible ledBlink(3); BT1.println("End 2nd Pass"); Serial.println("End 2nd Pass"); while (digitalRead(buttonPin) && !mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command = ""; } mode = STOPPED; status = 0; // 1st pass pathIndex = 0; pathLength = 0; } 

FunctionsC/C++

void mazeSolve(void) { while (!status) { readLFSsensors(); switch (mode) { case NO_LINE: motorStop(); goAndTurn (LEFT, 180); recIntersection('B'); break; case CONT_LINE: runExtraInch(); readLFSsensors(); if (mode != CONT_LINE) {goAndTurn (LEFT, 90); recIntersection('L');} // or it is a "T" or "Cross"). In both cases, goes to LEFT else mazeEnd(); break; case RIGHT_TURN: runExtraInch(); readLFSsensors(); if (mode == NO_LINE) {goAndTurn (RIGHT, 90); recIntersection('R');} else recIntersection('S'); break; case LEFT_TURN: goAndTurn (LEFT, 90); recIntersection('L'); break; case FOLLOWING_LINE: followingLine(); break; } } } //--------------------------------------------- void recIntersection(char direction) { path[pathLength] = direction; // Store the intersection in the path variable. pathLength ++; simplifyPath(); // Simplify the learned path. } //---------------------------------------------- void mazeEnd(void) { motorStop(); BT1.print("The End ==> Path: "); for(int i=0;i<pathLength;i++) BT1.print(path[i]); //Serial.print(path[i]); BT1.println(""); Serial.print(" pathLenght ==> "); Serial.println(pathLength); status = 1; mode = STOPPED; } //---------------------------------------------- void followingLine(void) { //readLFSsensors(); calculatePID(); motorPIDcontrol(); } //------------------------------------------------------------------------------------------ // Path simplification. The strategy is that whenever we encounter a // sequence xBx, we can simplify it by cutting out the dead end. For // example, LBL -> S, because a single S bypasses the dead end // represented by LBL. void simplifyPath() { // only simplify the path if the second-to-last turn was a 'B' if(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; break; case 'L': totalAngle += 270; break; case 'B': totalAngle += 180; break; } } // Get the angle as a number between 0 and 360 degrees. totalAngle = totalAngle % 360; // Replace all of those turns with a single one. switch(totalAngle) { case 0: path[pathLength - 3] = 'S'; break; case 90: path[pathLength - 3] = 'R'; break; case 180: path[pathLength - 3] = 'B'; break; case 270: path[pathLength - 3] = 'L'; break; } // The path is now two steps shorter. pathLength -= 2; } //---------------------------------------------------------------------------------------------- void mazeOptimization (void) { while (!status) { readLFSsensors(); switch (mode) { case FOLLOWING_LINE: followingLine(); break; case CONT_LINE: if (pathIndex >= pathLength) mazeEnd (); else {mazeTurn (path[pathIndex]); pathIndex++;} break; case LEFT_TURN: if (pathIndex >= pathLength) mazeEnd (); else {mazeTurn (path[pathIndex]); pathIndex++;} break; case RIGHT_TURN: if (pathIndex >= pathLength) mazeEnd (); else {mazeTurn (path[pathIndex]); pathIndex++;} break; } } } //----------------------------------------------------- void mazeTurn (char dir) { switch(dir) { case 'L': // Turn Left goAndTurn (LEFT, 90); break; case 'R': // Turn Right goAndTurn (RIGHT, 90); break; case 'B': // Turn Back goAndTurn (RIGHT, 800); break; case 'S': // Go Straight runExtraInch(); break; } } 

GeneralFunctions.C/C++

void ledBlink(int times) { for (int i = 0; i<times; i++) { digitalWrite (ledPin, HIGH); delay (500); digitalWrite (ledPin, LOW); delay (500); } } //----------------------------------------------------------------------------- void checkBTcmd() { while (BT1.available()) //Check if there is an available byte to read { delay(10); //Delay added to make thing stable char c = BT1.read(); //Conduct a serial read device += c; //build the string. } if (device.length() > 0) { Serial.print("Command received from BT ==> "); Serial.println(device); command = device; device =""; //Reset the variable BT1.flush(); } } //------------------------------------------------------------------------ void manualCmd() { switch (command[0]) { case 'g': mode = FOLLOWING_LINE; break; case 's': motorStop(); //turn off both motors break; case 'f': motorForward(); break; case 'r': motorTurn(RIGHT, 30); motorStop(); break; case 'l': motorTurn(LEFT, 30); motorStop(); break; case 'b': motorBackward(); break; case 'p': Kp = command[2]; break; case 'i': Ki = command[2]; break; case 'd': Kd = command[2]; break; } } //------------------------------------------------------------------------ void sendBTdata (int data) // send data to BT { digitalWrite (ledPin, HIGH); BT1.print("Data from Arduino"); BT1.print(data); BT1.print(" xx"); BT1.println('\n'); digitalWrite (ledPin, LOW); } //-------------------------------------------------------- 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); } 

sensorFuntionsC/C++

//------------------------------------------------------------- /* read line sensors values Sensor Array Error Value 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 found continuous line - test if an intersection or end of maze 0 0 0 0 0 0 Robot found no line: turn 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 )) {mode = CONT_LINE; error = 0;} else if((LFSensor[0]== 0 )&& (farRightSensor < THRESHOLD)) {mode = RIGHT_TURN; error = 0;} else if ((farLeftSensor < THRESHOLD) &&(LFSensor[4]== 0 )) {mode = LEFT_TURN; error = 0;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = NO_LINE; error = 0;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 4;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 3;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 2;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 1;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 0;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error =- 1;} else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -2;} else if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -3;} else if((LFSensor[0]== 1 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -4;} Serial.print (farLeftSensor); Serial.print (" <== LEFT RIGH==> "); Serial.print (farRightSensor); Serial.print (" mode: "); Serial.print (mode); Serial.print (" error:"); Serial.println (error); } //----------------------------------------------------------- void readLateralSensors(void) { } 

RobotDefines.hC/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 6 const 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 more to the Left 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; //Analog Pin A0 const int farLeftSensorPin = 1; //Analog Pin A0 const int THRESHOLD = 150; int farRightSensor = 0; int farLeftSensor = 0; int LFSensor[5]={0, 0, 0, 0, 0}; // PID controller float 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 -1 Servo leftServo; Servo rightServo; //------------------------------------------------- //Specific Maze Phase 2 (optimization) definitions and variables unsigned char dir; // The path variable will store the path that the robot has taken: // 'L' for left // 'R' for right // 'S' for straight (going straight through an intersection) // 'B' for back (U-turn) char path[100] = ""; unsigned char pathLength = 0; // the length of the path int pathIndex = 0; unsigned int status = 0; // solving = 0; reach end = 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 (unsigned int time) { motorForward(); delay (time); motorStop(); } //--------------------------------------------- void motorBwTime (unsigned int time) { motorBackward(); delay (time); motorStop(); } //------------------------------------------------ void motorTurn(int direction, int degrees) { leftServo.writeMicroseconds(1500 - (iniMotorPower+adj)*direction); rightServo.writeMicroseconds(1500 - iniMotorPower*direction); delay (round(adjTurn*degrees+20)); motorStop(); } //--------------------------------------------------- void motorPIDcontrol() { int leftMotorSpeed = 1500 - (iniMotorPower+adj) - PIDvalue; int rightMotorSpeed = 1500 + iniMotorPower - PIDvalue; // The motor speed should not exceed the max PWM value constrain(leftMotorSpeed, 1000, 2000); constrain(rightMotorSpeed, 1000, 2000); leftServo.writeMicroseconds(leftMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed); //Serial.print (PIDvalue); //Serial.print (" ==> Left, Right: "); //Serial.print (leftMotorSpeed); //Serial.print (" "); //Serial.println (rightMotorSpeed); } //--------------------------------------------------- void runExtraInch(void) { motorPIDcontrol(); delay(extraInch); motorStop(); } //--------------------------------------------------- void goAndTurn(int direction, int degrees) { motorPIDcontrol(); delay(adjGoAndTurn); motorTurn(direction, degrees); } 

Skematik