Scribd
Upload
60 views

PID+Line Follower Code

This document contains code for controlling a line following robot using a PID controller and Bluetooth communication. It initializes sensors and motors, calibrates the sensors, reads sensor values to determine line position for the PID controller, and allows adjusting PID parameters via Bluetooth.

Uploaded by

anju120485
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
60 views

PID+Line Follower Code

This document contains code for controlling a line following robot using a PID controller and Bluetooth communication. It initializes sensors and motors, calibrates the sensors, reads sensor values to determine line position for the PID controller, and allows adjusting PID parameters via Bluetooth.

Uploaded by

anju120485
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 8

#include <L298N.

h>

#include <QTRSensors.h>

#include "BluetoothSerial.h"

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)

#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it

#endif

#define AIN1 21 //Assign the motor pins

#define BIN1 25

#define AIN2 22

#define BIN2 33

#define PWMA 23

#define PWMB 32

#define STBY 19

const int offsetA = 1;

const int offsetB = 1;

// Initializing motors. The library will allow you to initialize as many

// motors as you have memory for. If you are using functions like forward

// that take 2 motors as arguements you can either write new functions or

// call the function more than once.

L298N motor1(PWMA, AIN1, AIN2);

L298N motor2(PWMB, BIN1, BIN2);

QTRSensors qtr;

BluetoothSerial SerialBT;
const uint8_t SensorCount = 5;

uint16_t sensorValues[SensorCount];

int threshold[SensorCount];

float Kp = 0;

float Ki = 0;

float Kd = 0;

uint8_t multiP = 1;

uint8_t multiI = 1;

uint8_t multiD = 1;

uint8_t Kpfinal;

uint8_t Kifinal;

uint8_t Kdfinal;

float Pvalue;

float Ivalue;

float Dvalue;

boolean onoff = false;

int val, cnt = 0, v[3];

uint16_t position;

int P, D, I, previousError, PIDvalue, error;

int lsp, rsp;

int lfspeed = 230;

void setup()

// configure the sensors

qtr.setTypeAnalog();
qtr.setSensorPins((const uint8_t[]){26, 27, 14, 12, 13}, SensorCount);

delay(500);

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(LED_BUILTIN, HIGH); // turn on Arduino's LED to indicate we are in calibration mode

Serial.begin(115200);

SerialBT.begin();

Serial.println("Bluetooth Started! Ready to pair...");

// analogRead() takes about 0.1 ms on an AVR.

// 0.1 ms per sensor * 4 samples per sensor read (default) * 6 sensors

// * 10 reads per calibrate() call = ~24 ms per calibrate() call.

// Call calibrate() 400 times to make calibration take about 10 seconds.

for (uint16_t i = 0; i < 400; i++)

qtr.calibrate();

digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with
calibration

// print the calibration minimum values measured when emitters were on

for (uint8_t i = 0; i < SensorCount; i++)

threshold[i] = (qtr.calibrationOn.minimum[i] + qtr.calibrationOn.maximum[i])/2;

Serial.print(threshold[i]);

Serial.print(" ");

Serial.println();

delay(1000);
}

void loop()

if (SerialBT.available()){

while(SerialBT.available() == 0);

valuesread();

processing();

if (onoff == true){

robot_control();

else if(onoff == false){

motor1.stop();

motor2.stop();

void robot_control(){

// read calibrated sensor values and obtain a measure of the line position

// from 0 to 4000 (for a white line, use readLineWhite() instead)

position = qtr.readLineBlack(sensorValues);

error = 2000 - position;

while(sensorValues[0]>=980 && sensorValues[1]>=980 && sensorValues[2]>=980 &&


sensorValues[3]>=980 && sensorValues[4]>=980){ // A case when the line follower leaves the line

if(previousError>0){ //Turn left if the line was to the left before

motor_drive(-230,230);

else{

motor_drive(230,-230); // Else turn right

position = qtr.readLineBlack(sensorValues);
}

PID_Linefollow(error);

//PID_Linefollow(error);

void PID_Linefollow(int error){

P = error;

I = I + error;

D = error - previousError;

Pvalue = (Kp/pow(10,multiP))*P;

Ivalue = (Ki/pow(10,multiI))*I;

Dvalue = (Kd/pow(10,multiD))*D;

float PIDvalue = Pvalue + Ivalue + Dvalue;

previousError = error;

lsp = lfspeed - PIDvalue;

rsp = lfspeed + PIDvalue;

if (lsp > 255) {

lsp = 255;

if (lsp < -255) {

lsp = -255;

if (rsp > 255) {

rsp = 255;

if (rsp < -255) {

rsp = -255;
}

motor_drive(lsp,rsp);

//This void delimits each instruction.

//The Arduino knows that for each instruction it will receive 2 bytes.

void valuesread() {

val = SerialBT.read();

cnt++;

v[cnt] = val;

if (cnt == 2)

cnt = 0;

//In this void the the 2 read values are assigned.

void processing() {

int a = v[1];

if (a == 1) {

Kp = v[2];

if (a == 2) {

multiP = v[2];

if (a == 3) {

Ki = v[2];

if (a == 4) {

multiI = v[2];

if (a == 5) {

Kd = v[2];

}
if (a == 6) {

multiD = v[2];

if (a == 7) {

onoff = v[2];

void motor_drive(int left, int right){

if(right>0)

motor2.setSpeed(right);

motor2.forward();

else

motor2.setSpeed(right);

motor2.backward();

if(left>0)

motor1.setSpeed(left);

motor1.forward();

else

motor1.setSpeed(left);

motor1.backward();

}
}

You might also like