Browse files

Added PID line follower functions

  • Loading branch information...
1 parent ee2aa82 commit b70b925e566be43c43f7ffee8040ddc6bcb66efa @robrighter committed May 19, 2012
Showing with 99 additions and 13 deletions.
  1. +1 −1 Makefile
  2. +2 −1 calibrate-for-line.c
  3. +1 −1 calibrate-for-line.h
  4. +68 −0 line-follower.c
  5. +10 −0 line-follower.h
  6. +17 −10 main.c
View
2 Makefile
@@ -14,7 +14,7 @@ PORT ?= /dev/tty.usbmodem00036221
AVRDUDE=avrdude
TARGET=main
-OBJECT_FILES=main.o calibrate-for-line.o
+OBJECT_FILES=main.o calibrate-for-line.o line-follower.o
all: $(TARGET).hex
View
3 calibrate-for-line.c
@@ -22,7 +22,7 @@ void calibrate_for_line(){
set_motors(0,0);
}
-void read_and_display_sensor_data(){
+unsigned int read_and_display_sensor_data(){
unsigned int values[NUMBER_OF_SENSORS];
unsigned int position = read_line(values,IR_EMITTERS_ON);
//print the position on the top line and the values on the 2nd line
@@ -35,4 +35,5 @@ void read_and_display_sensor_data(){
char c = displayable[values[i]/101];
print_character(c);
}
+ return position;
}
View
2 calibrate-for-line.h
@@ -1,2 +1,2 @@
void calibrate_for_line();
-void read_and_display_sensor_data();
+unsigned int read_and_display_sensor_data();
View
68 line-follower.c
@@ -0,0 +1,68 @@
+/////////////////////////////////////////////////////////
+// LINE FOLLOWER for 3pi robot
+// Uses a PID algo described here:
+// http://www.chibots.org/?q=node/339
+//
+// AUTHOR: @robrighter 2012
+////////////////////////////////////////////////////////
+
+#include <pololu/3pi.h>
+
+//Implementation of
+
+int last_proportional = 0;
+int integral = 0;
+
+//use these functions to tune the constants
+int apply_kp(int p){
+ return p/2;
+}
+
+int apply_kd(int d){
+ //For initial tuning we set the constant to 0
+ return 0*d;
+}
+
+int apply_ki(int i){
+ //For initial tuning we set the constant to 0
+ return 0*i;
+}
+
+int get_value_within_max(int value, int max){
+ if(value > max){
+ return max;
+ }
+ else if(value < (-max)){
+ return -max;
+ }
+ else{
+ return value;
+ }
+}
+
+void set_motors_by_position(int speed, unsigned int position){
+ //Calculate the PID
+ int proportional = ((int)position) - 2000;
+ integral += proportional;
+ int derivative = proportional - last_proportional;
+
+ //store off the proportional for future iteration
+ last_proportional = proportional;
+
+ //Actually calculate the speed.
+ //The equation is P(kp) + I(ki) + D(kd)
+ int d_speed = apply_kp(proportional) +
+ apply_ki(integral) +
+ apply_kd(derivative);
+
+ //make sure the numbers dont get to big
+ d_speed = get_value_within_max(d_speed, speed);
+
+ //set the motors
+ (d_speed< 0)?set_motors(d_speed,speed):set_motors(speed,d_speed);
+}
+
+void reset_line_follower(){
+ last_proportional = 0;
+ integral = 0;
+}
View
10 line-follower.h
@@ -0,0 +1,10 @@
+/////////////////////////////////////////////////////////
+// LINE FOLLOWER for 3pi robot
+// Uses a PID algo described here:
+// http://www.chibots.org/?q=node/339
+//
+// AUTHOR: @robrighter 2012
+////////////////////////////////////////////////////////
+
+void set_motors_by_position(int speed, unsigned int position);
+void reset_line_follower();
View
27 main.c
@@ -1,27 +1,34 @@
//#include <pololu/orangutan.h>
+#include <stdio.h>
#include <pololu/3pi.h>
#include "calibrate-for-line.h"
+#include "line-follower.h"
int main()
{
+ clear(); print("Ready");
while(1){
while(!button_is_pressed(BUTTON_B)){}
wait_for_button_release(BUTTON_B);
pololu_3pi_init(2000);
calibrate_for_line();
- read_and_display_sensor_data();
+
+ reset_line_follower();
+
+ //just follow the line for a little while while testing
+ int counter = 0;
+ while(counter++ < 330){
+ set_motors_by_position(30,read_and_display_sensor_data());
+ delay_ms(100);
+ }
+ set_motors(0,0);
}
//dont let the program ever reach the end of execution
- //or the robot may take over the world
+ //or the robot may take over the world. Keep this code at
+ //the bottom of the main just in case we screw up
+ while(1){ delay_ms(2000); }
return 0;
-}
-
-// Local Variables: **
-// mode: C **
-// c-basic-offset: 4 **
-// tab-width: 4 **
-// indent-tabs-mode: t **
-// end: **
+}

0 comments on commit b70b925

Please sign in to comment.