├── Line Follow App ├── line folllow app.png ├── MJRoBot_Line_Follower_PID_Control.aia └── MJRoBot_Line_Follower_PID_Control.apk ├── README.md └── smart_MJRoBot_Line_Follower_PID ├── robotDefines.h ├── motorFuntions.ino ├── smart_MJRoBot_Line_Follower_PID.ino ├── sensorFuntions.ino └── generalFunctions.ino /Line Follow App/line folllow app.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mjrovai/MJRoBot-Line-Follower/HEAD/Line Follow App/line folllow app.png -------------------------------------------------------------------------------- /Line Follow App/MJRoBot_Line_Follower_PID_Control.aia: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mjrovai/MJRoBot-Line-Follower/HEAD/Line Follow App/MJRoBot_Line_Follower_PID_Control.aia -------------------------------------------------------------------------------- /Line Follow App/MJRoBot_Line_Follower_PID_Control.apk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Mjrovai/MJRoBot-Line-Follower/HEAD/Line Follow App/MJRoBot_Line_Follower_PID_Control.apk -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MJRoBot-Line-Follower 2 | Line Follower Robot - PID Control - Android Setup 3 | 4 | Complete Project Instructions at: 5 | 6 | http://www.instructables.com/id/Line-Follower-Robot-PID-Control-Android-Setup/ 7 | -------------------------------------------------------------------------------- /smart_MJRoBot_Line_Follower_PID/robotDefines.h: -------------------------------------------------------------------------------- 1 | int mode = 0; 2 | 3 | # define STOPPED 0 4 | # define FOLLOWING_LINE 1 5 | # define NO_LINE 2 6 | 7 | const int power = 500; 8 | const int iniMotorPower = 250; 9 | const int adj = 1; 10 | float adjTurn = 8; 11 | 12 | const int ledPin = 13; 13 | const int buttonPin = 9; 14 | 15 | // LFSensor more to the Left is "0" 16 | const int lineFollowSensor0 = 12; 17 | const int lineFollowSensor1 = 18; 18 | const int lineFollowSensor2 = 17; 19 | const int lineFollowSensor3 = 16; 20 | const int lineFollowSensor4 = 19; 21 | 22 | int LFSensor[5]={0, 0, 0, 0, 0}; 23 | 24 | // PID controller 25 | float Kp=50; 26 | float Ki=0; 27 | float Kd=0; 28 | 29 | float error=0, P=0, I=0, D=0, PIDvalue=0; 30 | float previousError=0, previousI=0; 31 | 32 | #define RIGHT 1 33 | #define LEFT -1 34 | 35 | Servo leftServo; 36 | Servo rightServo; 37 | 38 | 39 | -------------------------------------------------------------------------------- /smart_MJRoBot_Line_Follower_PID/motorFuntions.ino: -------------------------------------------------------------------------------- 1 | 2 | void motorStop() 3 | { 4 | leftServo.writeMicroseconds(1500); 5 | rightServo.writeMicroseconds(1500); 6 | delay(200); 7 | } 8 | 9 | //--------------------------------------------- 10 | void motorForward() 11 | { 12 | leftServo.writeMicroseconds(1500 - power); 13 | rightServo.writeMicroseconds(1500 + power*adj); 14 | } 15 | 16 | //--------------------------------------------- 17 | void motorBackward() 18 | { 19 | leftServo.writeMicroseconds(1500 + power); 20 | rightServo.writeMicroseconds(1500 - power); 21 | } 22 | 23 | //--------------------------------------------- 24 | void motorFwTime (unsigned int time) 25 | { 26 | motorForward(); 27 | delay (time); 28 | motorStop(); 29 | } 30 | 31 | //--------------------------------------------- 32 | void motorBwTime (unsigned int time) 33 | { 34 | motorBackward(); 35 | delay (time); 36 | motorStop(); 37 | } 38 | 39 | //------------------------------------------------ 40 | void motorTurn(int direction, int degrees) 41 | { 42 | leftServo.writeMicroseconds(1500 - iniMotorPower*direction); 43 | rightServo.writeMicroseconds(1500 - iniMotorPower*direction); 44 | delay (round(adjTurn*degrees+1)); 45 | motorStop(); 46 | } 47 | 48 | //--------------------------------------------------- 49 | void motorPIDcontrol() 50 | { 51 | 52 | int leftMotorSpeed = 1500 - iniMotorPower - PIDvalue; 53 | int rightMotorSpeed = 1500 + iniMotorPower*adj - PIDvalue; 54 | 55 | // The motor speed should not exceed the max PWM value 56 | constrain(leftMotorSpeed, 1000, 2000); 57 | constrain(rightMotorSpeed, 1000, 2000); 58 | 59 | leftServo.writeMicroseconds(leftMotorSpeed); 60 | rightServo.writeMicroseconds(rightMotorSpeed); 61 | 62 | //Serial.print (PIDvalue); 63 | //Serial.print (" ==> Left, Right: "); 64 | //Serial.print (leftMotorSpeed); 65 | //Serial.print (" "); 66 | //Serial.println (rightMotorSpeed); 67 | } 68 | 69 | //----------------------------- 70 | void drivePolygon(unsigned int time, int sides) // for motor test only 71 | { 72 | for (int i = 0; i Basic movement based on Nano Mouse Robot, developed by Michael Backus (http://www.akrobotnerd.com/ ) 4 | ==> Line follow based on http://samvrit.tk/tutorials/pid-control-arduino-line-follower-robot/?ckattempt=1 5 | 6 | Marcelo Jose Rovai - 06 April, 2016 - Visit: http://mjrobot.org 7 | -------------------------------------------------------------------*/ 8 | 9 | #include 10 | #include "robotDefines.h" 11 | 12 | String command; 13 | String device; 14 | 15 | // BT Module 16 | #include 17 | SoftwareSerial BT1(10, 11); // El pin 10 es Rx y el pin 11 es Tx 18 | 19 | //--------------------------------------------- 20 | void setup() 21 | { 22 | 23 | Serial.begin(9600); 24 | BT1.begin(9600); 25 | 26 | pinMode(ledPin, OUTPUT); 27 | pinMode(buttonPin, INPUT_PULLUP); 28 | 29 | // line follow sensors 30 | pinMode(lineFollowSensor0, INPUT); 31 | pinMode(lineFollowSensor1, INPUT); 32 | pinMode(lineFollowSensor2, INPUT); 33 | pinMode(lineFollowSensor3, INPUT); 34 | pinMode(lineFollowSensor4, INPUT); 35 | 36 | // servos 37 | leftServo.attach(5); 38 | rightServo.attach(3); 39 | 40 | BT1.print("check the PID constants to be sent to Robot"); 41 | BT1.println('\n'); 42 | 43 | while (digitalRead(buttonPin) && !mode) 44 | { 45 | checkBTcmd(); // verify if a comand is received from BT remote control 46 | manualCmd (); 47 | command = ""; 48 | } 49 | checkPIDvalues(); 50 | mode = STOPPED; 51 | } 52 | 53 | void loop() 54 | { 55 | 56 | while (digitalRead(buttonPin) && !mode) 57 | { } 58 | 59 | readLFSsensors(); 60 | switch (mode) 61 | { 62 | case STOPPED: 63 | motorStop(); 64 | BT1.print("The End"); 65 | ledBlink(); 66 | previousError = error; 67 | break; 68 | 69 | case NO_LINE: 70 | motorStop(); 71 | motorTurn(LEFT, 180); 72 | previousError = 0; 73 | break; 74 | 75 | case FOLLOWING_LINE: 76 | calculatePID(); 77 | motorPIDcontrol(); 78 | break; 79 | } 80 | } 81 | 82 | 83 | -------------------------------------------------------------------------------- /smart_MJRoBot_Line_Follower_PID/sensorFuntions.ino: -------------------------------------------------------------------------------- 1 | 2 | 3 | //------------------------------------------------------------- 4 | /* read line sensors values 5 | 6 | Sensor Array Error Value 7 | 0 0 0 0 1 4 8 | 0 0 0 1 1 3 9 | 0 0 0 1 0 2 10 | 0 0 1 1 0 1 11 | 0 0 1 0 0 0 12 | 0 1 1 0 0 -1 13 | 0 1 0 0 0 -2 14 | 1 1 0 0 0 -3 15 | 1 0 0 0 0 -4 16 | 17 | 1 1 1 1 1 0 Robot found continuous line : STOPPED 18 | 0 0 0 0 0 0 Robot found no line: turn 180o 19 | 20 | */ 21 | void readLFSsensors() 22 | { 23 | LFSensor[0] = digitalRead(lineFollowSensor0); 24 | LFSensor[1] = digitalRead(lineFollowSensor1); 25 | LFSensor[2] = digitalRead(lineFollowSensor2); 26 | LFSensor[3] = digitalRead(lineFollowSensor3); 27 | LFSensor[4] = digitalRead(lineFollowSensor4); 28 | 29 | if(( LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 4;} 30 | else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) {mode = FOLLOWING_LINE; error = 3;} 31 | else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 2;} 32 | else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 1;} 33 | else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = 0;} 34 | else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error =- 1;} 35 | else if((LFSensor[0]== 0 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -2;} 36 | else if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -3;} 37 | else if((LFSensor[0]== 1 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = FOLLOWING_LINE; error = -4;} 38 | else if((LFSensor[0]== 1 )&&(LFSensor[1]== 1 )&&(LFSensor[2]== 1 )&&(LFSensor[3]== 1 )&&(LFSensor[4]== 1 )) {mode = STOPPED; error = 0;} 39 | else if((LFSensor[0]== 0 )&&(LFSensor[1]== 0 )&&(LFSensor[2]== 0 )&&(LFSensor[3]== 0 )&&(LFSensor[4]== 0 )) {mode = NO_LINE; error = 0;} 40 | } 41 | -------------------------------------------------------------------------------- /smart_MJRoBot_Line_Follower_PID/generalFunctions.ino: -------------------------------------------------------------------------------- 1 | void ledBlink(void) 2 | { 3 | for (int i = 0; i<4; i++) 4 | { 5 | digitalWrite (ledPin, HIGH); 6 | delay (500); 7 | digitalWrite (ledPin, LOW); 8 | delay (500); 9 | } 10 | } 11 | 12 | //----------------------------------------------------------------------------- 13 | 14 | void checkBTcmd() 15 | { 16 | while (BT1.available()) //Check if there is an available byte to read 17 | { 18 | delay(10); //Delay added to make thing stable 19 | char c = BT1.read(); //Conduct a serial read 20 | device += c; //build the string. 21 | } 22 | if (device.length() > 0) 23 | { 24 | Serial.print("Command received from BT ==> "); 25 | Serial.println(device); 26 | command = device; 27 | device =""; //Reset the variable 28 | BT1.flush(); 29 | } 30 | } 31 | 32 | //------------------------------------------------------------------------ 33 | void manualCmd() 34 | { 35 | switch (command[0]) 36 | { 37 | case 'g': 38 | mode = FOLLOWING_LINE; 39 | break; 40 | 41 | case 's': 42 | motorStop(); //turn off both motors 43 | break; 44 | 45 | case 'f': 46 | motorForward(); 47 | break; 48 | 49 | case 'r': 50 | motorTurn(RIGHT, 30); 51 | motorStop(); 52 | 53 | break; 54 | 55 | case 'l': 56 | motorTurn(LEFT, 30); 57 | motorStop(); 58 | break; 59 | 60 | case 'b': 61 | motorBackward(); 62 | break; 63 | 64 | case 'p': 65 | Kp = command[2]; 66 | break; 67 | 68 | case 'i': 69 | Ki = command[2]; 70 | break; 71 | 72 | case 'd': 73 | Kd = command[2]; 74 | break; 75 | } 76 | } 77 | 78 | //------------------------------------------------------------------------ 79 | void sendBTdata (int data) // send data to BT 80 | 81 | { 82 | digitalWrite (ledPin, HIGH); 83 | BT1.print("Data from Arduino"); 84 | BT1.print(data); 85 | BT1.print(" xx"); 86 | BT1.println('\n'); 87 | digitalWrite (ledPin, LOW); 88 | } 89 | 90 | //-------------------------------------------------------- 91 | void calculatePID() 92 | { 93 | P = error; 94 | I = I + error; 95 | D = error-previousError; 96 | PIDvalue = (Kp*P) + (Ki*I) + (Kd*D); 97 | previousError = error; 98 | } 99 | 100 | //-------------------------------------------------------- 101 | void checkPIDvalues() 102 | { 103 | 104 | BT1.print("PID: "); 105 | BT1.print(Kp); 106 | BT1.print(" - "); 107 | BT1.print(Ki); 108 | BT1.print(" - "); 109 | BT1.println(Kd); 110 | 111 | Serial.print("PID: "); 112 | Serial.print(Kp); 113 | Serial.print(" - "); 114 | Serial.print(Ki); 115 | Serial.print(" - "); 116 | Serial.println(Kd); 117 | 118 | } 119 | 120 | //----------------------------------------------- 121 | void testLineFollowSensors() 122 | { 123 | int LFS0 = digitalRead(lineFollowSensor0); 124 | int LFS1 = digitalRead(lineFollowSensor1); 125 | int LFS2 = digitalRead(lineFollowSensor2); 126 | int LFS3 = digitalRead(lineFollowSensor3); 127 | int LFS4 = digitalRead(lineFollowSensor4); 128 | 129 | Serial.print ("LFS: L 0 1 2 3 4 R ==> "); 130 | Serial.print (LFS0); 131 | Serial.print (" "); 132 | Serial.print (LFS1); 133 | Serial.print (" "); 134 | Serial.print (LFS2); 135 | Serial.print (" "); 136 | Serial.print (LFS3); 137 | Serial.print (" "); 138 | Serial.print (LFS4); 139 | Serial.print (" ==> "); 140 | 141 | Serial.print (" P: "); 142 | Serial.print (P); 143 | Serial.print (" I: "); 144 | Serial.print (I); 145 | Serial.print (" D: "); 146 | Serial.print (D); 147 | Serial.print (" PID: "); 148 | Serial.println (PIDvalue); 149 | } 150 | --------------------------------------------------------------------------------