My PID Line follwing code. What do you think :)

[size=12]working on line follower robotics competition. thought share have working. needs work it's working pretty far. input great.

-sam
[/size]

[size=10]/*
* enr-259
*/

//********************************************************************
// variables variables variables variables variables variables variabl
//********************************************************************
const int right_motor = 11;
const int left_motor = 10;
long sensors[] = {0, 0, 0, 0, 0};
long sensors_average = 0;
int sensors_sum = 0;
int position = 0;
int proportional = 0;
long integral = 0;
int derivative = 0;
int last_proportional = 0;
int control_value = 0;
int max_difference = 80;
float kp = .05;
float ki = .000001;
float kd = 1.105;

//********************************************************************
// setup function setup function setup function setup function setup f
//********************************************************************
void setup()
{
}

//********************************************************************
// loop function loop function loop function loop function loop functi
//********************************************************************
void loop()
{
 read_sensors();
 get_average();
 get_sum();
 get_position();
 get_proportional();
 get_integral();
 get_derivative();
 get_control();
 adjust_control();
 set_motors();
}

//********************************************************************
// read_sensors function read_sensors function read_sensors function r
//********************************************************************
void read_sensors()
{
 // take reading each of 5 line sensors
 for (int = 0; < 5; i++)
 {
   sensors = analogread(i);
   
   // round readings less 1 hundred down 0 filter
   // out surface noise
   if (sensors < 100)
   {
     sensors = 0;
   }
 }
}

//********************************************************************
// get_average function get_average function get_average function get_
//********************************************************************
void get_average()
{
 // 0 sensors_average variable
 sensors_average = 0;
 
 // generate weighted sensor average starting @ sensor 1 to
 // keep doing needless multiplicaiton 0 will
 // discarded value read sensor zero
 for (int = 1; < 5; ++)
 {
   sensors_average += sensors * * 1000;
 }
}

//********************************************************************
// get_sum function get_sum function get_sum function get_sum function
//********************************************************************
void get_sum()
{
 // 0 sensors_sum variable
 sensors_sum = 0;
 
 // sum total of sensor readings
 for (int = 0; < 5; i++)
 {
   sensors_sum += int(sensors);
 }
}

//********************************************************************
// get_position function get_position function get_position function g
//********************************************************************
void get_position()
{
 // calculate current position on line
 position = int(sensors_average / sensors_sum);
}

//********************************************************************
// get_proportional function get_proportional function get_proportiona
//********************************************************************
void get_proportional()
{
 // caculate proportional value
 proportional = position - 2000;
}

//********************************************************************
// get_integral function get_integral function get_integral function g
//********************************************************************
void get_integral()
{
 // calculate integral value
 integral = integral + proportional;
}

//********************************************************************
// get_derivative function get_derivative function get_derivative func
//********************************************************************
void get_derivative()
{
 // calculate derivative value
 derivative = proportional - last_proportional;
 
 // store proportional value in last_proportional derivative
 // calculation on next loop
 last_proportional = proportional;
}

//********************************************************************
// get_control function get_control function get_control function get_
//********************************************************************
void get_control()
{
 // calculate control value
 control_value = int(proportional * kp + integral * ki + derivative * kd);
}

//********************************************************************
// adjust_control function adjust_control function adjust_control func
//********************************************************************
void adjust_control()
{
 // if control value greater allowed maximum set it
 // maximum
 if (control_value > max_difference)
 {
   control_value = max_difference;
 }
 
 // if control value less allowed minimum set to
 // minimum
 if (control_value < -max_difference)
 {
   control_value = -max_difference;
 }
}

//********************************************************************
// set_motors function set_motors function set_motors function set_mot
//********************************************************************
void set_motors()
{
 // if control_value less 0 right turn needed to
 // acquire center of line
 if (control_value < 0)
 {
   analogwrite(right_motor, max_difference + control_value);
   analogwrite(left_motor, max_difference);
 }
 // if control_value greater 0 left turn needed to
 // acquire center of line
 else
 {
   analogwrite(right_motor, max_difference);
   analogwrite(left_motor, max_difference - control_value);
 }
}[/size]

added in directional control of motors today making quick 90deg turns. added flagging function note when perpendicular lines crossed turning routine , other changes. feel free ask questions or tell me how might things.

note: if of pid values seem different (much higher) last code it's because switched voltage regulators. running 16 volts required me pulse motors low values reasonable speed. found got greater control switching 8 volt regulator , using whole pwm range.

code: [select]

/*
* enr-259
*/

//********************************************************************
// variables variables variables variables variables variables variabl
//********************************************************************
const int right_motor = 11;
const int left_motor = 10;
const int right_forward = 2;
const int right_reverse = 3;
const int left_forward = 4;
const int left_reverse = 5;
int right_speed = 0;
int left_speed = 0;
long sensors[] = {0, 0, 0, 0, 0};
long sensors_average = 0;
int sensors_sum = 0;
boolean sensors_flag = false;
int line_count = 0;
int position = 0;
int proportional = 0;
long integral = 0;
int derivative = 0;
int last_proportional = 0;
int control_value = 0;
int max_difference = 222;
float kp = .111;
float ki = 0;
float kd = 1;

//********************************************************************
// setup function setup function setup function setup function setup f
//********************************************************************
void setup()
{
 // test test test test test test test test test test test test test
 serial.begin(9600);
 
 // setup motor direction control pins
 pinmode(right_forward, output);
 pinmode(right_reverse, output);
 pinmode(left_forward, output);
 pinmode(left_reverse, output);
 
 // initialize both motors forward
 digitalwrite(right_forward, high);
 digitalwrite(right_reverse, low);
 digitalwrite(left_forward, high);
 digitalwrite(left_reverse, low);
}

//********************************************************************
// loop function loop function loop function loop function loop functi
//********************************************************************
void loop()
{
 read_sensors();
 get_average();
 get_sum();
 get_flags();
 process_flags();
 get_position();
 get_proportional();
 get_integral();
 get_derivative();
 get_control();
 process_control();
 set_motors(right_speed, left_speed);
}

//********************************************************************
// read_sensors function read_sensors function read_sensors function r
//********************************************************************
void read_sensors()
{
 // take reading each of 5 line sensors
 for (int = 0; < 5; i++)
 {
   sensors[i] = analogread(i);
   
   // round readings less 2 hundred fifty down 0 filter
   // out surface noise
   if (sensors[i] < 250)
   {
     sensors[i] = 0;
   }
 }
}

//********************************************************************
// get_average function get_average function get_average function get_
//********************************************************************
void get_average()
{
 // 0 sensors_average variable
 sensors_average = 0;
 
 // generate weighted sensor average starting @ sensor 1 to
 // keep doing needless multiplicaiton 0 will
 // discarded value read sensor zero
 for (int = 1; < 5; ++)
 {
   sensors_average += sensors[i] * * 1000;
 }
}

//********************************************************************
// get_sum function get_sum function get_sum function get_sum function
//********************************************************************
void get_sum()
{
 // 0 sensors_sum variable
 sensors_sum = 0;
 
 // sum total of sensor readings
 for (int = 0; < 5; i++)
 {
   sensors_sum += int(sensors[i]);
 }
}

//********************************************************************
// get_flags function get_flags function get_flags function get_flags
//********************************************************************
void get_flags()
{
 // if perpendicular line read set sensors_flag and
 // increment line count
 if (sensors_sum > 4450)
 {
   sensors_flag = true;
   line_count++;
 }
}

//********************************************************************
// process_flags process_flags process_flags process_flags process_fla
//********************************************************************
void process_flags()
{
 if (sensors_flag == true && line_count == 2)
 {
   // make right turn , acquire center of line
   right_turn();
 }
 else if (sensors_flag == true && line_count == 3)
 {
   // add code here
 }
 else if (sensors_flag == true && line_count == 4)
 {
   // add code here
 }
 
 // clear sensors_flag
 sensors_flag = false;
}

//********************************************************************
// get_position function get_position function get_position function g
//********************************************************************
void get_position()
{
 // calculate current position on line
 position = int(sensors_average / sensors_sum);
}

//********************************************************************
// get_proportional function get_proportional function get_proportiona
//********************************************************************
void get_proportional()
{
 // caculate proportional value
 proportional = position - 2000;
}

//********************************************************************
// get_integral function get_integral function get_integral function g
//********************************************************************
void get_integral()
{
 // calculate integral value
 integral = integral + proportional;
}

//********************************************************************
// get_derivative function get_derivative function get_derivative func
//********************************************************************
void get_derivative()
{
 // calculate derivative value
 derivative = proportional - last_proportional;
 
 // store proportional value in last_proportional derivative
 // calculation on next loop
 last_proportional = proportional;
}

//********************************************************************
// get_control function get_control function get_control function get_
//********************************************************************
void get_control()
{
 // calculate control value
 control_value = int(proportional * kp +
                     integral * ki +
                     derivative * kd);
}

//********************************************************************
// process_control process_control process_control process_control pro
//********************************************************************
void process_control()
{
 // if control value greater allowed maximum set it
 // maximum
 if (control_value > max_difference)
 {
   control_value = max_difference;
 }
 
 // if control value less allowed minimum set to
 // minimum
 if (control_value < -max_difference)
 {
   control_value = -max_difference;
 }
 
 // if control_value less 0 calculate right turn to
 // acquire center of line
 if (control_value < 0)
 {
   right_speed = max_difference + control_value;
   left_speed = max_difference;
 }
 // if control_value greater 0 calculate left turn to
 // acquire center of line
 else
 {
   right_speed = max_difference;
   left_speed = max_difference - control_value;
 }
}

//********************************************************************
// set_motors function set_motors function set_motors function set_mot
//********************************************************************
void set_motors(int right_speed, int left_speed)
{
 // adjust motor speeds according calculated values
 analogwrite(right_motor, right_speed);
 analogwrite(left_motor, left_speed);
}

//********************************************************************
// right_turn function right_turn function right_turn function right_t
//********************************************************************
void right_turn()
{
 // cut power left , right motors , coast until
 // perpendicular line passed
 set_motors(0, 0);
 delay(250);
 
 // take new sensor 0 reading
 sensors[0] = analogread(0);
 
 // initialize right motor reverse travel
 digitalwrite(right_forward, low);
 digitalwrite(right_reverse, high);
 
 // turn right until edge of line acquired
 while (sensors[0] < 500)
 {
   set_motors(222, 222);
   sensors[0] = analogread(0);
 }
 
 // initialize right motor forward travel
 digitalwrite(right_forward, high);
 digitalwrite(right_reverse, low);
}

Arduino Forum > Forum 2005-2010 (read only) > Software > Syntax & Programs > My PID Line follwing code. What do you think :)

arduino