Elliot the Line Follower Robot

A PID controlled line follower robot, named Elliot.

Dec 13, 2016

•

32213 views

•

16 respects

environmental sensing

robot

tracking

robotics

Devices & Components

Arduino Uno Rev3

Dual H-Bridge motor drivers L298

6V NiMh 2000mAh rechargeable battery

Robot Chassis

Painter's stick

DC motor (generic)

Solderless Breadboard Half Size

Electrical tape

9V battery (generic)

9V Battery Clip

Jumper wires (generic)

Caster wheel

IR Sensors

Angle Bracket

Project description

Code

PID Line Follower Robot Code

c_cpp

1float pTerm, iTerm, dTerm;
2int error;
3int previousError;
4float  kp = 11; //11
5float ki = 0;
6float kd = 11; //11
7float output;
8int integral,  derivative;
9int irSensors[] = {13, 12, 11, 8, 7}; //IR sensor pins
10int irReadings[5];
11int  motor1Forward = A0;
12int motor1Backward = A1;
13int motor1pwmPin = 5;
14int  motor2Forward = A3;
15int motor2Backward = A2;
16int motor2pwmPin = 3;
17int  motor1newSpeed;
18int motor2newSpeed;
19int motor2Speed = 70; //Default 70
20int  motor1Speed = 120; //Default 120
21
22void setup() {
23  //Declare all IR sensors  as inputs
24  for (int pin = 0; pin < 5; pin++) {
25    int pinNum = irSensors[pin];
26    pinMode(pinNum, INPUT);
27  }
28  pinMode(motor1Forward, OUTPUT);
29  pinMode(motor1Backward,  OUTPUT);
30  pinMode(motor1pwmPin, OUTPUT);
31  pinMode(motor2Forward, OUTPUT);
32  pinMode(motor2Backward, OUTPUT);
33  pinMode(motor2pwmPin, OUTPUT);
34}
35
36void  loop() {
37  //Put all of our functions here
38  readIRSensors();
39  calculateError();
40  pidCalculations();
41  changeMotorSpeed();
42}
43
44void readIRSensors() {
45  //Read the IR sensors and put the readings in irReadings array
46  for (int pin  = 0; pin < 5; pin++) {
47    int pinNum = irSensors[pin];
48    irReadings[pin]  = digitalRead(pinNum);
49  }
50}
51
52void calculateError() {
53  //Determine  an error based on the readings
54  if ((irReadings[0] == 0) && (irReadings[1] ==  0) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 1)) {
55    error = 4;
56  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2]  == 0) && (irReadings[3] == 1) && (irReadings[4] == 1)) {
57    error = 3;
58  }  else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0) &&  (irReadings[3] == 1) && (irReadings[4] == 0)) {
59    error = 2;
60  } else if  ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 1) && (irReadings[3]  == 1) && (irReadings[4] == 0)) {
61    error = 1;
62  } else if ((irReadings[0]  == 0) && (irReadings[1] == 0) && (irReadings[2] == 1) && (irReadings[3] == 0) &&  (irReadings[4] == 0)) {
63    error = 0;
64  } else if ((irReadings[0] == 0) &&  (irReadings[1] == 1) && (irReadings[2] == 1) && (irReadings[3] == 0) && (irReadings[4]  == 0)) {
65    error = -1;
66  } else if ((irReadings[0] == 0) && (irReadings[1]  == 1) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {
67    error = -2;
68  } else if ((irReadings[0] == 1) && (irReadings[1] == 1) &&  (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {
69    error  = -3;
70  } else if ((irReadings[0] == 1) && (irReadings[1] == 0) && (irReadings[2]  == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {
71    error = -4;
72  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0)  && (irReadings[3] == 0) && (irReadings[4] == 0)) {
73    if (previousError == -4)  {
74      error = -5;
75    } else {
76      error = 5;
77    }
78  } else  if ((irReadings[0] == 1) && (irReadings[1] == 1) && (irReadings[2] == 1) && (irReadings[3]  == 1) && (irReadings[4] == 1)) {
79    error = 0;
80  }
81}
82
83void pidCalculations()  {
84  pTerm = kp * error;
85  integral += error;
86  iTerm = ki * integral;
87  derivative = error - previousError;
88  dTerm = kd * derivative;
89  output  = pTerm + iTerm + dTerm;
90  previousError = error;
91}
92
93void changeMotorSpeed()  {
94  //Change motor speed of both motors accordingly
95  motor2newSpeed = motor2Speed  + output;
96  motor1newSpeed = motor1Speed - output;
97  //Constrain the new speed  of motors to be between the range 0-255
98  constrain(motor2newSpeed, 0, 255);
99  constrain(motor1newSpeed, 0, 255);
100  //Set new speed, and run motors in forward  direction
101  analogWrite(motor2pwmPin, motor2newSpeed);
102  analogWrite(motor1pwmPin,  motor1newSpeed);
103  digitalWrite(motor2Forward, HIGH);
104  digitalWrite(motor2Backward,  LOW);
105  digitalWrite(motor1Forward, HIGH);
106  digitalWrite(motor1Backward,  LOW);
107}
108
109

float pTerm, iTerm, dTerm;
int error;
int previousError;
float  kp = 11; //11
float ki = 0;
float kd = 11; //11
float output;
int integral,  derivative;
int irSensors[] = {13, 12, 11, 8, 7}; //IR sensor pins
int irReadings[5];
int  motor1Forward = A0;
int motor1Backward = A1;
int motor1pwmPin = 5;
int  motor2Forward = A3;
int motor2Backward = A2;
int motor2pwmPin = 3;
int  motor1newSpeed;
int motor2newSpeed;
int motor2Speed = 70; //Default 70
int  motor1Speed = 120; //Default 120

void setup() {
  //Declare all IR sensors  as inputs
  for (int pin = 0; pin < 5; pin++) {
    int pinNum = irSensors[pin];
    pinMode(pinNum, INPUT);
  }
  pinMode(motor1Forward, OUTPUT);
  pinMode(motor1Backward,  OUTPUT);
  pinMode(motor1pwmPin, OUTPUT);
  pinMode(motor2Forward, OUTPUT);
  pinMode(motor2Backward, OUTPUT);
  pinMode(motor2pwmPin, OUTPUT);
}

void  loop() {
  //Put all of our functions here
  readIRSensors();
  calculateError();
  pidCalculations();
  changeMotorSpeed();
}

void readIRSensors() {
  //Read the IR sensors and put the readings in irReadings array
  for (int pin  = 0; pin < 5; pin++) {
    int pinNum = irSensors[pin];
    irReadings[pin]  = digitalRead(pinNum);
  }
}

void calculateError() {
  //Determine  an error based on the readings
  if ((irReadings[0] == 0) && (irReadings[1] ==  0) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 1)) {
    error = 4;
  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2]  == 0) && (irReadings[3] == 1) && (irReadings[4] == 1)) {
    error = 3;
  }  else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0) &&  (irReadings[3] == 1) && (irReadings[4] == 0)) {
    error = 2;
  } else if  ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 1) && (irReadings[3]  == 1) && (irReadings[4] == 0)) {
    error = 1;
  } else if ((irReadings[0]  == 0) && (irReadings[1] == 0) && (irReadings[2] == 1) && (irReadings[3] == 0) &&  (irReadings[4] == 0)) {
    error = 0;
  } else if ((irReadings[0] == 0) &&  (irReadings[1] == 1) && (irReadings[2] == 1) && (irReadings[3] == 0) && (irReadings[4]  == 0)) {
    error = -1;
  } else if ((irReadings[0] == 0) && (irReadings[1]  == 1) && (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {
    error = -2;
  } else if ((irReadings[0] == 1) && (irReadings[1] == 1) &&  (irReadings[2] == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {
    error  = -3;
  } else if ((irReadings[0] == 1) && (irReadings[1] == 0) && (irReadings[2]  == 0) && (irReadings[3] == 0) && (irReadings[4] == 0)) {
    error = -4;
  } else if ((irReadings[0] == 0) && (irReadings[1] == 0) && (irReadings[2] == 0)  && (irReadings[3] == 0) && (irReadings[4] == 0)) {
    if (previousError == -4)  {
      error = -5;
    } else {
      error = 5;
    }
  } else  if ((irReadings[0] == 1) && (irReadings[1] == 1) && (irReadings[2] == 1) && (irReadings[3]  == 1) && (irReadings[4] == 1)) {
    error = 0;
  }
}

void pidCalculations()  {
  pTerm = kp * error;
  integral += error;
  iTerm = ki * integral;
  derivative = error - previousError;
  dTerm = kd * derivative;
  output  = pTerm + iTerm + dTerm;
  previousError = error;
}

void changeMotorSpeed()  {
  //Change motor speed of both motors accordingly
  motor2newSpeed = motor2Speed  + output;
  motor1newSpeed = motor1Speed - output;
  //Constrain the new speed  of motors to be between the range 0-255
  constrain(motor2newSpeed, 0, 255);
  constrain(motor1newSpeed, 0, 255);
  //Set new speed, and run motors in forward  direction
  analogWrite(motor2pwmPin, motor2newSpeed);
  analogWrite(motor1pwmPin,  motor1newSpeed);
  digitalWrite(motor2Forward, HIGH);
  digitalWrite(motor2Backward,  LOW);
  digitalWrite(motor1Forward, HIGH);
  digitalWrite(motor1Backward,  LOW);
}

Downloadable files

PID Line Follower Robot Schematic

Sorry if this is not the best diagram/schematic, and if you have any questions about the IR sensor wiring I'd be happy to help. The reason for this is I had trouble finding IR sensor modules on Fritzing.

PID Line Follower Robot Schematic

Comments

Only logged in users can leave comments

SMM2

0 Followers

•

0 Projects

Intro