Components and supplies
Solarbotics RW2i Wheel
5V Step-Up/Step-Down Voltage Regulator S7V7F5
QTR-8RC Reflectance Sensor Array
HC-05 Bluetooth Module
Micro Metal Gearmotor HPCB 6V
Button 6x6x6
Li-Po Battery 7.4 V
Ball Caster with 3/8″ Metal Ball
DRV8835 Dual Motor Driver Carrier
Resistor 1k ohm
Arduino Pro Mini 328 - 5V/16MHz
Tools and machines
Soldering iron (generic)
3D Printer (generic)
Apps and platforms
SketchUp
Arduino IDE
EasyEDA
MIT App Inventor 2
Project description
Code
QTR-8RC Example
c_cpp
The Polulu modified example of the QTR-8RC sensor array
1//Made by POLULU 2 3#include <QTRSensors.h> 4 5// This example is designed for use with eight RC QTR sensors. These 6// reflectance sensors should be connected to digital pins 3 to 10. The 7// sensors' emitter control pin (CTRL or LEDON) can optionally be connected to 8// digital pin 2, or you can leave it disconnected and remove the call to 9// setEmitterPin(). 10// 11// The setup phase of this example calibrates the sensors for ten seconds and 12// turns on the Arduino's LED (usually on pin 13) while calibration is going 13// on. During this phase, you should expose each reflectance sensor to the 14// lightest and darkest readings they will encounter. For example, if you are 15// making a line follower, you should slide the sensors across the line during 16// the calibration phase so that each sensor can get a reading of how dark the 17// line is and how light the ground is. Improper calibration will result in 18// poor readings. 19// 20// The main loop of the example reads the calibrated sensor values and uses 21// them to estimate the position of a line. You can test this by taping a piece 22// of 3/4" black electrical tape to a piece of white paper and sliding the 23// sensor across it. It prints the sensor values to the serial monitor as 24// numbers from 0 (maximum reflectance) to 1000 (minimum reflectance) followed 25// by the estimated location of the line as a number from 0 to 5000. 1000 means 26// the line is directly under sensor 1, 2000 means directly under sensor 2, 27// etc. 0 means the line is directly under sensor 0 or was last seen by sensor 28// 0 before being lost. 5000 means the line is directly under sensor 5 or was 29// last seen by sensor 5 before being lost. 30 31QTRSensors qtr; 32 33const uint8_t SensorCount = 8; 34uint16_t sensorValues[SensorCount]; 35 36void setup() 37{ 38 // configure the sensors 39 qtr.setTypeRC(); 40 qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount); 41 qtr.setEmitterPin(7); 42 43 delay(500); 44 pinMode(LED_BUILTIN, OUTPUT); 45 digitalWrite(LED_BUILTIN, HIGH); // turn on Arduino's LED to indicate we are in calibration mode 46 47 // 2.5 ms RC read timeout (default) * 10 reads per calibrate() call 48 // = ~25 ms per calibrate() call. 49 // Call calibrate() 400 times to make calibration take about 10 seconds. 50 for (uint16_t i = 0; i < 400; i++) 51 { 52 qtr.calibrate(); 53 } 54 digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with calibration 55 56 // print the calibration minimum values measured when emitters were on 57 Serial.begin(9600); 58 for (uint8_t i = 0; i < SensorCount; i++) 59 { 60 Serial.print(qtr.calibrationOn.minimum[i]); 61 Serial.print(' '); 62 } 63 Serial.println(); 64 65 // print the calibration maximum values measured when emitters were on 66 for (uint8_t i = 0; i < SensorCount; i++) 67 { 68 Serial.print(qtr.calibrationOn.maximum[i]); 69 Serial.print(' '); 70 } 71 Serial.println(); 72 Serial.println(); 73 delay(1000); 74} 75 76void loop() 77{ 78 // read calibrated sensor values and obtain a measure of the line position 79 // from 0 to 5000 (for a white line, use readLineWhite() instead) 80 uint16_t position = qtr.readLineBlack(sensorValues); 81 82 // print the sensor values as numbers from 0 to 1000, where 0 means maximum 83 // reflectance and 1000 means minimum reflectance, followed by the line 84 // position 85 for (uint8_t i = 0; i < SensorCount; i++) 86 { 87 Serial.print(sensorValues[i]); 88 Serial.print('\ '); 89 } 90 Serial.println(position); 91 92 delay(250); 93}
Advanced PID control + Bluetooth communication
c_cpp
You can change the rotation of the motors (only forward or forward-backward).
1#include <QTRSensors.h> 2QTRSensors qtr; 3const uint8_t SensorCount = 8; 4uint16_t sensorValues[SensorCount]; 5 6int lastError = 0; 7boolean onoff = 0; 8int val, cnt = 0, v[3]; 9 10const uint16_t threshold = 500; // adjustable - can take values between 0 and 1000 11 12//the speed can be between 0 and 255 - 0 is LOW and 255 is HIGH. At a high value, 13//you may risk burning the motors, because the voltage supplied by the PWM control 14//is higher than 6V. 15const int maxspeeda = 150; 16const int maxspeedb = 150; 17const int basespeeda = 100; 18const int basespeedb = 100; 19/*If your robot can't take tight curves, you can set up the robot 20 to revolve around the base (and not around one of the wheels) by 21 setting the minspeed to a negative value (-100), so the motors will go 22 forward and backward. Doing this the motors can wear out faster. 23 If you don't want to do this, set the minspeed to 0, so the motors 24 will only go forward. 25*/ 26//const int minspeeda = 0; 27//const int minspeedb = 0; 28const int minspeeda = -100; 29const int minspeedb = -100; 30 31float Kp = 0; 32float Ki = 0; 33float Kd = 0; 34uint8_t multiP = 1; 35uint8_t multiI = 1; 36uint8_t multiD = 1; 37uint8_t Kpfinal; 38uint8_t Kifinal; 39uint8_t Kdfinal; 40int P; 41int I; 42int D; 43float Pvalue; 44float Ivalue; 45float Dvalue; 46 47int mode = 8; 48int aphase = 9; 49int aenbl = 6; 50int bphase = 5; 51int benbl = 3; 52 53void setup() { 54 Serial.begin(9600); 55 qtr.setTypeRC(); 56 qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount); 57 qtr.setEmitterPin(7); 58 59 pinMode(mode, OUTPUT); 60 pinMode(aphase, OUTPUT); 61 pinMode(aenbl, OUTPUT); 62 pinMode(bphase, OUTPUT); 63 pinMode(benbl, OUTPUT); 64 digitalWrite(mode, HIGH); 65 66 delay(500); 67 pinMode(LED_BUILTIN, OUTPUT); 68 calibration(); 69 forward_brake(0, 0); 70} 71 72void calibration() { 73 digitalWrite(LED_BUILTIN, HIGH); 74 for (uint16_t i = 0; i < 400; i++) 75 { 76 qtr.calibrate(); 77 } 78 digitalWrite(LED_BUILTIN, LOW); 79} 80 81void loop() { 82 if (Serial.available()) { 83 while(Serial.available() == 0); 84 valuesread(); 85 processing(); 86 } 87 if(onoff == 1) { 88 robot_control(); 89 } 90 if(onoff == 0) { 91 forward_brake(0, 0); 92 } 93} 94 95//This void delimits each instruction. 96//The Arduino knows that for each instruction it will receive 2 bytes. 97void valuesread() { 98 val = Serial.read(); 99 cnt++; 100 v[cnt] = val; 101 if (cnt == 2) 102 cnt = 0; 103} 104 105//In this void the the 2 read values are assigned. 106void processing() { 107 int a = v[1]; 108 if (a == 1) { 109 Kp = v[2]; 110 } 111 if (a == 2) { 112 multiP = v[2]; 113 } 114 if (a == 3) { 115 Ki = v[2]; 116 } 117 if (a == 4) { 118 multiI = v[2]; 119 } 120 if (a == 5) { 121 Kd = v[2]; 122 } 123 if (a == 6) { 124 multiD = v[2]; 125 } 126 if (a == 7) { 127 onoff = v[2]; 128 } 129} 130 131//Make sure that this values are assigned correctly 132void forward_brake(int posa, int posb) { 133 digitalWrite(aphase, LOW); 134 digitalWrite(bphase, HIGH); 135 analogWrite(aenbl, posa); 136 analogWrite(benbl, posb); 137} 138void left_brake(int posa, int posb) { 139 digitalWrite(aphase, LOW); 140 digitalWrite(bphase, LOW); 141 analogWrite(aenbl, posa); 142 analogWrite(benbl, posb); 143} 144void right_brake(int posa, int posb) { 145 digitalWrite(aphase, HIGH); 146 digitalWrite(bphase, HIGH); 147 analogWrite(aenbl, posa); 148 analogWrite(benbl, posb); 149} 150 151void robot_control() { 152 uint16_t position = qtr.readLineBlack(sensorValues); 153 int error = 3500 - position; 154 155 int cnt = 0; 156 float sum = 0; 157 for (int i = 0; i < 8; i++) { 158 if(sensorValues[i] >= threshold) { 159 cnt++; 160 sum = sum + i; 161 } 162 } 163 164 165 //I made a case where the robot can cross the line (when the curve is too tight) and the position can be 3500 166 //even though it is not on the center of the line. If you don't want your motors to rotate in 2 directions 167 //comment the right_brake(100,100) / left_brake(100, 100) and uncomment the forward_brake(0,100) / forward_brake(0,100) 168 169 170 if (cnt >= 3) { 171 int motorspeeda = 0; 172 int motorspeedb = 0; 173 int val = sum/cnt; 174 if(val < 3.5) { 175 //turn right 176 right_brake(100, 100); 177 //forward_brake(0,100); 178 } 179 if(val > 3.5) { 180 //turn left 181 left_brake(100, 100); 182 //forward_brake(100,0); 183 } 184 if(val == 3.5) { 185 cnt = cnt/2; 186 uint16_t mini = 1000; 187 uint8_t minpos = 0; 188 for (int i = 4 - cnt; i <= 3 + cnt; i++) { 189 if (mini > sensorValues[i]) { 190 mini = sensorValues[i]; 191 minpos = i; 192 } 193 } 194 if(minpos < 3.5) { 195 //turn right 196 right_brake(100, 100); 197 //forward_brake(0,100); 198 } 199 if(minpos > 3.5) { 200 //turn left 201 left_brake(100, 100); 202 //forward_brake(100,0); 203 } 204 } 205 } 206 else { 207 PID(error); 208 } 209} 210 211void PID(int error) { 212 int P = error; 213 int I = I + error; 214 int D = error - lastError; 215 lastError = error; 216 Pvalue = (Kp/pow(10,multiP))*P; 217 Ivalue = (Ki/pow(10,multiI))*I; 218 Dvalue = (Kd/pow(10,multiD))*D; 219 220 float motorspeed = Pvalue + Ivalue + Dvalue; 221 222 int motorspeeda = basespeeda + motorspeed; 223 int motorspeedb = basespeedb - motorspeed; 224 225 if (motorspeeda > maxspeeda) { 226 motorspeeda = maxspeeda; 227 } 228 if (motorspeedb > maxspeedb) { 229 motorspeedb = maxspeedb; 230 } 231 if (motorspeeda < minspeeda) { 232 motorspeeda = minspeeda; 233 } 234 if (motorspeedb < minspeedb) { 235 motorspeedb = minspeedb; 236 } 237 //Serial.print(motorspeeda); Serial.print(" "); Serial.println(motorspeedb); 238 speedcontrol(motorspeeda, motorspeedb); 239} 240 241void speedcontrol(int mota, int motb) { 242 if (mota >= 0 && motb >= 0) { 243 forward_brake(mota, motb); 244 } 245 if (mota < 0 && motb >= 0) { 246 //dreapta 247 mota = 0 - mota; 248 right_brake(mota, motb); 249 250 } 251 if (mota >= 0 && motb < 0) { 252 //stanga 253 motb = 0 - motb; 254 left_brake(mota, motb); 255 } 256} 257
PID_LF_example.ino
arduino
The basic PID control system algorithm implemented with the specified hardware.
1/* 2 * File name: PID_LF_example 3 * 4 * Hardware requirements: an Arduino Pro Mini 5 * a QTR-8RC Reflectance Sensor Array 6 * a DRV8835 Dual Motor Driver Carrier 7 * 8 * Description: The basic PID control system implemented with 9 * the line follower with the specified hardware. 10 * The robot can follow a black line on a white surface 11 * (or vice versa). 12 * Related Document: See the written documentation or the LF video from 13 * Bot Reboot. 14 * 15 * Author: Bot Reboot 16 */ 17 18#include <QTRSensors.h> //Make sure to install the library 19 20/************************************************************************* 21* Sensor Array object initialisation 22*************************************************************************/ 23QTRSensors qtr; 24const uint8_t SensorCount = 8; 25uint16_t sensorValues[SensorCount]; 26 27/************************************************************************* 28* PID control system variables 29*************************************************************************/ 30float Kp = 0; //related to the proportional control term; 31 //change the value by trial-and-error (ex: 0.07). 32float Ki = 0; //related to the integral control term; 33 //change the value by trial-and-error (ex: 0.0008). 34float Kd = 0; //related to the derivative control term; 35 //change the value by trial-and-error (ex: 0.6). 36int P; 37int I; 38int D; 39 40/************************************************************************* 41* Global variables 42*************************************************************************/ 43int lastError = 0; 44boolean onoff = false; 45 46/************************************************************************* 47* Motor speed variables (choose between 0 - no speed, and 255 - maximum speed) 48*************************************************************************/ 49const uint8_t maxspeeda = 150; 50const uint8_t maxspeedb = 150; 51const uint8_t basespeeda = 100; 52const uint8_t basespeedb = 100; 53 54/************************************************************************* 55* DRV8835 GPIO pins declaration 56*************************************************************************/ 57int mode = 8; 58int aphase = 9; 59int aenbl = 6; 60int bphase = 5; 61int benbl = 3; 62 63/************************************************************************* 64* Buttons pins declaration 65*************************************************************************/ 66int buttoncalibrate = 17; //or pin A3 67int buttonstart = 2; 68 69/************************************************************************* 70* Function Name: setup 71************************************************************************** 72* Summary: 73* This is the setup function for the Arduino board. It first sets up the 74* pins for the sensor array and the motor driver. Then the user needs to 75* slide the sensors across the line for 10 seconds as they need to be 76* calibrated. 77* 78* Parameters: 79* none 80* 81* Returns: 82* none 83*************************************************************************/ 84void setup() { 85 qtr.setTypeRC(); 86 qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount); 87 qtr.setEmitterPin(7);//LEDON PIN 88 89 pinMode(mode, OUTPUT); 90 pinMode(aphase, OUTPUT); 91 pinMode(aenbl, OUTPUT); 92 pinMode(bphase, OUTPUT); 93 pinMode(benbl, OUTPUT); 94 digitalWrite(mode, HIGH); //one of the two control interfaces 95 //(simplified drive/brake operation) 96 delay(500); 97 pinMode(LED_BUILTIN, OUTPUT); 98 99 boolean Ok = false; 100 while (Ok == false) { // the main function won't start until the robot is calibrated 101 if(digitalRead(buttoncalibrate) == HIGH) { 102 calibration(); //calibrate the robot for 10 seconds 103 Ok = true; 104 } 105 } 106 forward_brake(0, 0); //stop the motors 107} 108 109/************************************************************************* 110* Function Name: calibration 111************************************************************************** 112* Summary: 113* This is the calibration function for the QTR-8RC Reflectance Sensor Array. 114* The function calls the method 'qtr.calibrate()' offered by the imported 115* library. For approx. 10 seconds, each of the 8 sensors will calibrate with 116* readings from the track. 117* 118* Parameters: 119* none 120* 121* Returns: 122* none 123*************************************************************************/ 124void calibration() { 125 digitalWrite(LED_BUILTIN, HIGH); 126 for (uint16_t i = 0; i < 400; i++) 127 { 128 qtr.calibrate(); 129 } 130 digitalWrite(LED_BUILTIN, LOW); 131} 132 133/************************************************************************* 134* Function Name: loop 135************************************************************************** 136* Summary: 137* This is the main function of this application. When the start button is 138* pressed, the robot will toggle between following the track and stopping. 139* When following the track, the function calls the PID control method. 140* 141* Parameters: 142* none 143* 144* Returns: 145* none 146*************************************************************************/ 147void loop() { 148 if(digitalRead(buttonstart) == HIGH) { 149 onoff =! onoff; 150 if(onoff = true) { 151 delay(1000);//a delay when the robot starts 152 } 153 else { 154 delay(50); 155 } 156 } 157 if (onoff == true) { 158 PID_control(); 159 } 160 else { 161 forward_brake(0,0); //stop the motors 162 } 163} 164 165/************************************************************************* 166* Function Name: forward_brake 167************************************************************************** 168* Summary: 169* This is the control interface function of the motor driver. As shown in 170* the Pololu's documentation of the DRV8835 motor driver, when the MODE is 171* equal to 1 (the pin is set to output HIGH), the robot will go forward at 172* the given speed specified by the parameters. The phase pins control the 173* direction of the spin, and the enbl pins control the speed of the motor. 174* 175* A warning though, depending on the wiring, you might need to change the 176* aphase and bphase from LOW to HIGH, in order for the robot to spin forward. 177* 178* Parameters: 179* int posa: int value from 0 to 255; controls the speed of the motor A. 180* int posb: int value from 0 to 255; controls the speed of the motor B. 181* 182* Returns: 183* none 184*************************************************************************/ 185void forward_brake(int posa, int posb) { 186 //set the appropriate values for aphase and bphase so that the robot goes straight 187 digitalWrite(aphase, LOW); 188 digitalWrite(bphase, LOW); 189 analogWrite(aenbl, posa); 190 analogWrite(benbl, posb); 191} 192 193/************************************************************************* 194* Function Name: PID_control 195************************************************************************** 196* Summary: 197* This is the function of the PID control system. The distinguishing 198* feature of the PID controller is the ability to use the three control 199* terms of proportional, integral and derivative influence on the controller 200* output to apply accurate and optimal control. This correction is applied to 201* the speed of the motors, which should be in range of the interval [0, max_speed], 202* max_speed <= 255. 203* 204* Parameters: 205* none 206* 207* Returns: 208* none 209*************************************************************************/ 210void PID_control() { 211 uint16_t position = qtr.readLineBlack(sensorValues); //read the current position 212 int error = 3500 - position; //3500 is the ideal position (the centre) 213 214 P = error; 215 I = I + error; 216 D = error - lastError; 217 lastError = error; 218 int motorspeed = P*Kp + I*Ki + D*Kd; //calculate the correction 219 //needed to be applied to the speed 220 221 int motorspeeda = basespeeda + motorspeed; 222 int motorspeedb = basespeedb - motorspeed; 223 224 if (motorspeeda > maxspeeda) { 225 motorspeeda = maxspeeda; 226 } 227 if (motorspeedb > maxspeedb) { 228 motorspeedb = maxspeedb; 229 } 230 if (motorspeeda < 0) { 231 motorspeeda = 0; 232 } 233 if (motorspeedb < 0) { 234 motorspeedb = 0; 235 } 236 forward_brake(motorspeeda, motorspeedb); 237} 238
Code used in Arduino Code Development #1
arduino
Checkout the video I made using this code, to understand its functionality: https://youtu.be/w3bE3ygJMx8
1#include <QTRSensors.h> 2 3int aphase = 9; 4int aenlb = 6; 5int bphase = 5; 6int benbl = 3; 7int mode = 8; 8 9int P; 10int I; 11int D; 12 13float Kp = 0.05; 14float Ki = 0.00001; 15float Kd = 0.8; 16 17int lastError = 0; 18 19int button_calibration = A3; 20int button_start = 2; 21 22QTRSensors qtr; 23 24const uint8_t SensorCount = 8; 25uint16_t sensorValues[SensorCount]; 26 27void setup() { 28 // put your setup code here, to run once: 29 qtr.setTypeRC(); 30 qtr.setSensorPins((const uint8_t[]){10, 11, 12, A0, A1, A2, A4, A5}, SensorCount); 31 qtr.setEmitterPin(7); 32 33 pinMode(aphase, OUTPUT); 34 pinMode(aenlb, OUTPUT); 35 pinMode(bphase, OUTPUT); 36 pinMode(benbl, OUTPUT); 37 pinMode(mode, OUTPUT); 38 39 pinMode(button_calibration, INPUT); 40 pinMode(button_start, INPUT); 41 42 digitalWrite(mode, HIGH); 43 44 while(digitalRead(button_calibration) == LOW) {} 45 //10 seconds 46 for (uint16_t i = 0; i < 400; i++) 47 { 48 qtr.calibrate(); 49 } 50 51 while(digitalRead(button_start) == LOW) {} 52} 53 54void loop() { 55 // put your main code here, to run repeatedly: 56 PID_control(); 57} 58 59void PID_control() { 60 uint16_t positionLine = qtr.readLineBlack(sensorValues); 61 int error = 3500 - positionLine; 62 63 P = error; 64 I = error + I; 65 D = error - lastError; 66 lastError = error; 67 68 int motorSpeedChange = P*Kp + I*Ki + D*Kd; 69 70 int motorSpeedA = 100 + motorSpeedChange; 71 int motorSpeedB = 100 - motorSpeedChange; 72 73 if (motorSpeedA > 125) { 74 motorSpeedA = 125; 75 } 76 if (motorSpeedB > 125) { 77 motorSpeedB = 125; 78 } 79 if (motorSpeedA < -75) { 80 motorSpeedA = -75; 81 } 82 if (motorSpeedB < -75) { 83 motorSpeedB = -75; 84 } 85 forward_movement(motorSpeedA, motorSpeedB); 86} 87 88void forward_movement(int speedA, int speedB) { 89 if (speedA < 0) { 90 speedA = 0 - speedA; 91 digitalWrite(aphase, LOW); 92 } 93 else { 94 digitalWrite(aphase, HIGH); 95 } 96 if (speedB < 0) { 97 speedB = 0 - speedB; 98 digitalWrite(bphase, HIGH); 99 } 100 else { 101 digitalWrite(bphase, LOW); 102 } 103 analogWrite(aenlb, speedA); 104 analogWrite(benbl, speedB); 105}
Advanced PID control + Bluetooth communication
c_cpp
You can change the rotation of the motors (only forward or forward-backward).
1#include <QTRSensors.h> 2QTRSensors qtr; 3const uint8_t SensorCount 4 = 8; 5uint16_t sensorValues[SensorCount]; 6 7int lastError = 0; 8boolean 9 onoff = 0; 10int val, cnt = 0, v[3]; 11 12const uint16_t threshold = 500; // 13 adjustable - can take values between 0 and 1000 14 15//the speed can be between 16 0 and 255 - 0 is LOW and 255 is HIGH. At a high value, 17//you may risk burning 18 the motors, because the voltage supplied by the PWM control 19//is higher than 20 6V. 21const int maxspeeda = 150; 22const int maxspeedb = 150; 23const int basespeeda 24 = 100; 25const int basespeedb = 100; 26/*If your robot can't take tight curves, 27 you can set up the robot 28 to revolve around the base (and not around one of 29 the wheels) by 30 setting the minspeed to a negative value (-100), so the motors 31 will go 32 forward and backward. Doing this the motors can wear out faster. 33 34 If you don't want to do this, set the minspeed to 0, so the motors 35 will 36 only go forward. 37*/ 38//const int minspeeda = 0; 39//const int minspeedb = 40 0; 41const int minspeeda = -100; 42const int minspeedb = -100; 43 44float Kp 45 = 0; 46float Ki = 0; 47float Kd = 0; 48uint8_t multiP = 1; 49uint8_t multiI 50 = 1; 51uint8_t multiD = 1; 52uint8_t Kpfinal; 53uint8_t Kifinal; 54uint8_t Kdfinal; 55int 56 P; 57int I; 58int D; 59float Pvalue; 60float Ivalue; 61float Dvalue; 62 63int 64 mode = 8; 65int aphase = 9; 66int aenbl = 6; 67int bphase = 5; 68int benbl = 69 3; 70 71void setup() { 72 Serial.begin(9600); 73 qtr.setTypeRC(); 74 qtr.setSensorPins((const 75 uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount); 76 qtr.setEmitterPin(7); 77 78 79 pinMode(mode, OUTPUT); 80 pinMode(aphase, OUTPUT); 81 pinMode(aenbl, OUTPUT); 82 83 pinMode(bphase, OUTPUT); 84 pinMode(benbl, OUTPUT); 85 digitalWrite(mode, 86 HIGH); 87 88 delay(500); 89 pinMode(LED_BUILTIN, OUTPUT); 90 calibration(); 91 92 forward_brake(0, 0); 93} 94 95void calibration() { 96 digitalWrite(LED_BUILTIN, 97 HIGH); 98 for (uint16_t i = 0; i < 400; i++) 99 { 100 qtr.calibrate(); 101 102 } 103 digitalWrite(LED_BUILTIN, LOW); 104} 105 106void loop() { 107 if (Serial.available()) 108 { 109 while(Serial.available() == 0); 110 valuesread(); 111 processing(); 112 113 } 114 if(onoff == 1) { 115 robot_control(); 116 } 117 if(onoff == 0) { 118 119 forward_brake(0, 0); 120 } 121} 122 123//This void delimits each instruction. 124//The 125 Arduino knows that for each instruction it will receive 2 bytes. 126void valuesread() 127 { 128 val = Serial.read(); 129 cnt++; 130 v[cnt] = val; 131 if (cnt == 2) 132 133 cnt = 0; 134} 135 136//In this void the the 2 read values are assigned. 137void 138 processing() { 139 int a = v[1]; 140 if (a == 1) { 141 Kp = v[2]; 142 } 143 144 if (a == 2) { 145 multiP = v[2]; 146 } 147 if (a == 3) { 148 Ki = v[2]; 149 150 } 151 if (a == 4) { 152 multiI = v[2]; 153 } 154 if (a == 5) { 155 Kd 156 = v[2]; 157 } 158 if (a == 6) { 159 multiD = v[2]; 160 } 161 if (a == 7) 162 { 163 onoff = v[2]; 164 } 165} 166 167//Make sure that this values are assigned 168 correctly 169void forward_brake(int posa, int posb) { 170 digitalWrite(aphase, 171 LOW); 172 digitalWrite(bphase, HIGH); 173 analogWrite(aenbl, posa); 174 analogWrite(benbl, 175 posb); 176} 177void left_brake(int posa, int posb) { 178 digitalWrite(aphase, LOW); 179 180 digitalWrite(bphase, LOW); 181 analogWrite(aenbl, posa); 182 analogWrite(benbl, 183 posb); 184} 185void right_brake(int posa, int posb) { 186 digitalWrite(aphase, 187 HIGH); 188 digitalWrite(bphase, HIGH); 189 analogWrite(aenbl, posa); 190 analogWrite(benbl, 191 posb); 192} 193 194void robot_control() { 195 uint16_t position = qtr.readLineBlack(sensorValues); 196 197 int error = 3500 - position; 198 199 int cnt = 0; 200 float sum = 0; 201 for 202 (int i = 0; i < 8; i++) { 203 if(sensorValues[i] >= threshold) { 204 cnt++; 205 206 sum = sum + i; 207 } 208 } 209 210 211 //I made a case where the robot 212 can cross the line (when the curve is too tight) and the position can be 3500 213 214 //even though it is not on the center of the line. If you don't want your motors 215 to rotate in 2 directions 216 //comment the right_brake(100,100) / left_brake(100, 217 100) and uncomment the forward_brake(0,100) / forward_brake(0,100) 218 219 220 221 if (cnt >= 3) { 222 int motorspeeda = 0; 223 int motorspeedb = 0; 224 int 225 val = sum/cnt; 226 if(val < 3.5) { 227 //turn right 228 right_brake(100, 229 100); 230 //forward_brake(0,100); 231 } 232 if(val > 3.5) { 233 //turn 234 left 235 left_brake(100, 100); 236 //forward_brake(100,0); 237 } 238 239 if(val == 3.5) { 240 cnt = cnt/2; 241 uint16_t mini = 1000; 242 uint8_t 243 minpos = 0; 244 for (int i = 4 - cnt; i <= 3 + cnt; i++) { 245 if (mini 246 > sensorValues[i]) { 247 mini = sensorValues[i]; 248 minpos 249 = i; 250 } 251 } 252 if(minpos < 3.5) { 253 //turn right 254 255 right_brake(100, 100); 256 //forward_brake(0,100); 257 } 258 259 if(minpos > 3.5) { 260 //turn left 261 left_brake(100, 100); 262 263 //forward_brake(100,0); 264 } 265 } 266 } 267 else { 268 PID(error); 269 270 } 271} 272 273void PID(int error) { 274 int P = error; 275 int I = I + error; 276 277 int D = error - lastError; 278 lastError = error; 279 Pvalue = (Kp/pow(10,multiP))*P; 280 281 Ivalue = (Ki/pow(10,multiI))*I; 282 Dvalue = (Kd/pow(10,multiD))*D; 283 284 float 285 motorspeed = Pvalue + Ivalue + Dvalue; 286 287 int motorspeeda = basespeeda + 288 motorspeed; 289 int motorspeedb = basespeedb - motorspeed; 290 291 if (motorspeeda 292 > maxspeeda) { 293 motorspeeda = maxspeeda; 294 } 295 if (motorspeedb > maxspeedb) 296 { 297 motorspeedb = maxspeedb; 298 } 299 if (motorspeeda < minspeeda) { 300 301 motorspeeda = minspeeda; 302 } 303 if (motorspeedb < minspeedb) { 304 motorspeedb 305 = minspeedb; 306 } 307 //Serial.print(motorspeeda); Serial.print(" "); Serial.println(motorspeedb); 308 309 speedcontrol(motorspeeda, motorspeedb); 310} 311 312void speedcontrol(int mota, 313 int motb) { 314 if (mota >= 0 && motb >= 0) { 315 forward_brake(mota, motb); 316 317 } 318 if (mota < 0 && motb >= 0) { 319 //dreapta 320 mota = 0 - mota; 321 322 right_brake(mota, motb); 323 324 } 325 if (mota >= 0 && motb < 0) { 326 327 //stanga 328 motb = 0 - motb; 329 left_brake(mota, motb); 330 } 331} 332
PID_LF_example.ino
arduino
The basic PID control system algorithm implemented with the specified hardware.
1/* 2 * File name: PID_LF_example 3 * 4 * Hardware requirements: 5 an Arduino Pro Mini 6 * a QTR-8RC Reflectance Sensor Array 7 8 * a DRV8835 Dual Motor Driver Carrier 9 * 10 11 * Description: The basic PID control system implemented with 12 * the 13 line follower with the specified hardware. 14 * The robot can follow 15 a black line on a white surface 16 * (or vice versa). 17 * Related 18 Document: See the written documentation or the LF video from 19 * Bot 20 Reboot. 21 * 22 * Author: Bot Reboot 23 */ 24 25#include 26 <QTRSensors.h> //Make sure to install the library 27 28/************************************************************************* 29* 30 Sensor Array object initialisation 31*************************************************************************/ 32QTRSensors 33 qtr; 34const uint8_t SensorCount = 8; 35uint16_t sensorValues[SensorCount]; 36 37/************************************************************************* 38* 39 PID control system variables 40*************************************************************************/ 41float 42 Kp = 0; //related to the proportional control term; 43 //change the 44 value by trial-and-error (ex: 0.07). 45float Ki = 0; //related to the integral 46 control term; 47 //change the value by trial-and-error (ex: 0.0008). 48float 49 Kd = 0; //related to the derivative control term; 50 //change the 51 value by trial-and-error (ex: 0.6). 52int P; 53int I; 54int D; 55 56/************************************************************************* 57* 58 Global variables 59*************************************************************************/ 60int 61 lastError = 0; 62boolean onoff = false; 63 64/************************************************************************* 65* 66 Motor speed variables (choose between 0 - no speed, and 255 - maximum speed) 67*************************************************************************/ 68const 69 uint8_t maxspeeda = 150; 70const uint8_t maxspeedb = 150; 71const uint8_t basespeeda 72 = 100; 73const uint8_t basespeedb = 100; 74 75/************************************************************************* 76* 77 DRV8835 GPIO pins declaration 78*************************************************************************/ 79int 80 mode = 8; 81int aphase = 9; 82int aenbl = 6; 83int bphase = 5; 84int benbl = 85 3; 86 87/************************************************************************* 88* 89 Buttons pins declaration 90*************************************************************************/ 91int 92 buttoncalibrate = 17; //or pin A3 93int buttonstart = 2; 94 95/************************************************************************* 96* 97 Function Name: setup 98************************************************************************** 99* 100 Summary: 101* This is the setup function for the Arduino board. It first sets up 102 the 103* pins for the sensor array and the motor driver. Then the user needs to 104 105* slide the sensors across the line for 10 seconds as they need to be 106* 107 calibrated. 108* 109* Parameters: 110* none 111* 112* Returns: 113* none 114*************************************************************************/ 115void 116 setup() { 117 qtr.setTypeRC(); 118 qtr.setSensorPins((const uint8_t[]){10, 11, 119 12, 14, 15, 16, 18, 19}, SensorCount); 120 qtr.setEmitterPin(7);//LEDON PIN 121 122 123 pinMode(mode, OUTPUT); 124 pinMode(aphase, OUTPUT); 125 pinMode(aenbl, OUTPUT); 126 127 pinMode(bphase, OUTPUT); 128 pinMode(benbl, OUTPUT); 129 digitalWrite(mode, 130 HIGH); //one of the two control interfaces 131 //(simplified 132 drive/brake operation) 133 delay(500); 134 pinMode(LED_BUILTIN, OUTPUT); 135 136 137 boolean Ok = false; 138 while (Ok == false) { // the main function won't start 139 until the robot is calibrated 140 if(digitalRead(buttoncalibrate) == HIGH) { 141 142 calibration(); //calibrate the robot for 10 seconds 143 Ok = true; 144 145 } 146 } 147 forward_brake(0, 0); //stop the motors 148} 149 150/************************************************************************* 151* 152 Function Name: calibration 153************************************************************************** 154* 155 Summary: 156* This is the calibration function for the QTR-8RC Reflectance Sensor 157 Array. 158* The function calls the method 'qtr.calibrate()' offered by the imported 159 160* library. For approx. 10 seconds, each of the 8 sensors will calibrate with 161* 162 readings from the track. 163* 164* Parameters: 165* none 166* 167* Returns: 168* 169 none 170*************************************************************************/ 171void 172 calibration() { 173 digitalWrite(LED_BUILTIN, HIGH); 174 for (uint16_t i = 0; 175 i < 400; i++) 176 { 177 qtr.calibrate(); 178 } 179 digitalWrite(LED_BUILTIN, 180 LOW); 181} 182 183/************************************************************************* 184* 185 Function Name: loop 186************************************************************************** 187* 188 Summary: 189* This is the main function of this application. When the start button 190 is 191* pressed, the robot will toggle between following the track and stopping. 192* 193 When following the track, the function calls the PID control method. 194* 195* 196 Parameters: 197* none 198* 199* Returns: 200* none 201*************************************************************************/ 202void 203 loop() { 204 if(digitalRead(buttonstart) == HIGH) { 205 onoff =! onoff; 206 207 if(onoff = true) { 208 delay(1000);//a delay when the robot starts 209 210 } 211 else { 212 delay(50); 213 } 214 } 215 if (onoff == true) 216 { 217 PID_control(); 218 } 219 else { 220 forward_brake(0,0); //stop the 221 motors 222 } 223} 224 225/************************************************************************* 226* 227 Function Name: forward_brake 228************************************************************************** 229* 230 Summary: 231* This is the control interface function of the motor driver. As shown 232 in 233* the Pololu's documentation of the DRV8835 motor driver, when the MODE is 234 235* equal to 1 (the pin is set to output HIGH), the robot will go forward at 236* 237 the given speed specified by the parameters. The phase pins control the 238* direction 239 of the spin, and the enbl pins control the speed of the motor. 240* 241* A warning 242 though, depending on the wiring, you might need to change the 243* aphase and bphase 244 from LOW to HIGH, in order for the robot to spin forward. 245* 246* Parameters: 247* 248 int posa: int value from 0 to 255; controls the speed of the motor A. 249* int 250 posb: int value from 0 to 255; controls the speed of the motor B. 251* 252* Returns: 253* 254 none 255*************************************************************************/ 256void 257 forward_brake(int posa, int posb) { 258 //set the appropriate values for aphase 259 and bphase so that the robot goes straight 260 digitalWrite(aphase, LOW); 261 digitalWrite(bphase, 262 LOW); 263 analogWrite(aenbl, posa); 264 analogWrite(benbl, posb); 265} 266 267/************************************************************************* 268* 269 Function Name: PID_control 270************************************************************************** 271* 272 Summary: 273* This is the function of the PID control system. The distinguishing 274 275* feature of the PID controller is the ability to use the three control 276* 277 terms of proportional, integral and derivative influence on the controller 278* 279 output to apply accurate and optimal control. This correction is applied to 280* 281 the speed of the motors, which should be in range of the interval [0, max_speed], 282* 283 max_speed <= 255. 284* 285* Parameters: 286* none 287* 288* Returns: 289* none 290*************************************************************************/ 291void 292 PID_control() { 293 uint16_t position = qtr.readLineBlack(sensorValues); //read 294 the current position 295 int error = 3500 - position; //3500 is the ideal position 296 (the centre) 297 298 P = error; 299 I = I + error; 300 D = error - lastError; 301 302 lastError = error; 303 int motorspeed = P*Kp + I*Ki + D*Kd; //calculate the correction 304 305 //needed to be applied to the speed 306 307 308 int motorspeeda = basespeeda + motorspeed; 309 int motorspeedb = basespeedb - 310 motorspeed; 311 312 if (motorspeeda > maxspeeda) { 313 motorspeeda = maxspeeda; 314 315 } 316 if (motorspeedb > maxspeedb) { 317 motorspeedb = maxspeedb; 318 } 319 320 if (motorspeeda < 0) { 321 motorspeeda = 0; 322 } 323 if (motorspeedb < 0) 324 { 325 motorspeedb = 0; 326 } 327 forward_brake(motorspeeda, motorspeedb); 328} 329
Downloadable files
Gerber File
This ZIP contains all the files for the fabrication of the PCB shown.
Gerber File
Schematic Line Follower Fritzing
Schematic Line Follower Fritzing
PID controller
App for phone to control and set up the values for the line follower
PID controller
Schematic Line Follower EasyEDA
Schematic Line Follower EasyEDA
PID controller
App for phone to control and set up the values for the line follower
PID controller
Schematic Line Follower Fritzing
Schematic Line Follower Fritzing
Gerber File
This ZIP contains all the files for the fabrication of the PCB shown.
Gerber File
Schematic Line Follower EasyEDA
Schematic Line Follower EasyEDA
Documentation
STL new parts
Contains updated STL parts for the robot (the battery holder has a shorter length while the motor bracket is longer). It also features a battery holder cover). Read the ReadMe file for more details about the 3d printing (material, quantity)
STL new parts
CAD Line Follower
Download this STL file to get an idea of how to assemble the robot. You can find the STL parts in a ZIP in the same section.
CAD Line Follower
STL new parts
Contains updated STL parts for the robot (the battery holder has a shorter length while the motor bracket is longer). It also features a battery holder cover). Read the ReadMe file for more details about the 3d printing (material, quantity)
STL new parts
CAD Line Follower
Download this STL file to get an idea of how to assemble the robot. You can find the STL parts in a ZIP in the same section.
CAD Line Follower
STL parts
Read the ReadMe file for more details about the 3d printing (material, quantity)
STL parts
Comments
Only logged in users can leave comments
anova9347
0 Followers
•0 Projects
Table of contents
Intro
25
0