Skip to content

Commit

Permalink
fix to Walter's PID algorithm, added a more simple version
Browse files Browse the repository at this point in the history
  • Loading branch information
plusk01 committed Mar 2, 2015
1 parent 8796f52 commit 372af85
Show file tree
Hide file tree
Showing 3 changed files with 143 additions and 15 deletions.
5 changes: 4 additions & 1 deletion README.md
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
Line Follower Resources
=======================


Resources for the Line Follower Competition, Winter 2015

There are two versions of the line follower algorithm in this repo:
- A simple control algorithm with two sensors that just switches direction ([code](https://github.com/byu-mechatronics/line-follower/blob/master/line_follower_bang.ino)).
- A more advanced control system with a PID loop ([code](https://github.com/byu-mechatronics/line-follower/blob/master/line_follower_one.ino)).
137 changes: 137 additions & 0 deletions line_follower_bang.ino
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
/*
Line Follower control using Bang-Bang algorithm
Written 3/2/15 by Walter Coe, BYU Mechatronics Club
For more explaination on Bang-Bang control, see here...
http://en.wikipedia.org/wiki/Bang%E2%80%93bang_control
Ecplaination for use of this code...
Use two sensors positioned so that they straddle the line to be followed
The assumed sensor is a photo-transistor sensor (i.e. QRD1114 or similar)
There must be a threshold determined by the user, i.e. the value of variable 's_thresh'
Builder must verify that the sensor theshold is not met by both sensors simultaniously
*/

//Define Sensor Pins
//A0 is on the left
//A1 is on the right
const int s0 = A0;
const int s1 = A1;


//Motor Constraints
//Set motor_low to lowest duty cycle % that turns the motor
const float motor_low = .25;
const float motor_avg = .75;

//Define motor drive pins
//Must be chosen for your wiring, connect these pins to switches(MOSFET of BJT)
const int motor_L = 3;
const int motor_R = 4;

//Define the sensor threshold
const float s_thresh = .25;

//Variable used to to store sensor data
int turn;

//Holds time of last line detection
//Used to avoid runaway robots
long t_last;
const int t_thresh = 10000;
const int pause = 10000;

//----------------------------------------------------------------------------------------------------------------------------

void setup()
{
//Setup serial terminal printing
Serial.begin(9600);

//Setup the pins that will drive the switches (MOSFET or BJT)
pinMode(motor_L, OUTPUT);
pinMode(motor_R, OUTPUT);

//Initialize
turn = 0;

//Initialize
t_last = millis();
}

void loop()
{
//Check for the line
turn = line_check(s0, s1);

//Based on sensor reading, turn or go straight
drive_motor(turn, motor_L, motor_R, motor_low, motor_avg);

//If you saw the line, reset timer
if( turn != 0)
{
t_last = millis();
}

//If the line hasn't been seen, stop for some time
if( millis() - t_last > t_thresh )
{
delay(pause);
}

}

//----------------------------------------------------------------------------------------------------------------------------

int line_check(int s0, int s1)
{

//Line seen by left sensor
if(analogRead(s0) <= s_thresh)
{
return(0);
}

//Line seen by right sensor
else if(analogRead(s1) <= s_thresh)
{
return(1);
}

//Line not seen
else
{
return(-1);
}

}

void drive_motor(int turn_dir, int motor_L, int motor_R, float motor_low, float motor_avg)
{
//Turn right
if( turn_dir == 0 )
{
analogWrite(motor_L, motor_avg);
analogWrite(motor_R, motor_low);
}

//Turn left
else if( turn_dir == 1)
{
analogWrite(motor_L, motor_low);
analogWrite(motor_R, motor_avg);
}

//Go straight
else
{
analogWrite(motor_L, motor_avg);
analogWrite(motor_R, motor_avg);
}

}



16 changes: 2 additions & 14 deletions line_follower_one.ino
Original file line number Diff line number Diff line change
@@ -1,22 +1,15 @@
/*
Line Follower utilizing a PID algorithm
Written 2/28/15 by Walter Coe, BYU Mechatronics Club
A great explanation of PID can be found here...
http://www.pc-control.co.uk/feedback_control.htm
- -----------> +
|-| s |-|
| |------------| |
|-| ||| |-|
|||
|||
Assumptions for using this code....
Using one photo-transistor sensor (i.e. QRD1114 or similar)
The sensor will need to be tuned, by choosing the pull-up resistor
A bigger resistor will make for a faster transition between 0-5v output, but will result in a narrow detection range
Expand All @@ -29,13 +22,8 @@ http://www.pc-control.co.uk/feedback_control.htm
2) Connect power (can be done with a switch)
3) Arduino will wait 2 seconds before starting to follow the line
4) If the Arduino moves to fast to react to the line, you can slow the speed by lowering the motor_avg value
sensor reads light (higher value output from 'pos' equation) when it needs to turn left
sensor reads dark (lower value output from 'pos' equation) where it needs to turn right
*/

//Define Sensor Pins
Expand Down Expand Up @@ -146,7 +134,7 @@ void loop()

//To far left when negative
//To far right when positive
differentiator = (2*tau - Ts)/(2*tau+Ts)*differentiator + (2/(2*tau+Ts))*(error_d1 - error);
differentiator = (2*tau - Ts)/(2*tau+Ts)*differentiator + (2/(2*tau+Ts))*(posError_d1 - posError);


//If Integrating is safe
Expand Down Expand Up @@ -257,4 +245,4 @@ void drive_motor(float PID, float PID_max, float pwm_min, float pwm_avg, int mot
Serial.println(PWM_L);
Serial.println(PWM_R);

}
}

0 comments on commit 372af85

Please sign in to comment.