Working on a line follower for a robotics competition. Just thought I would share what I have working. Needs much work but it's working pretty well so far. Any input would be great.
-Sam
/*
- 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 a reading from each of the five line sensors
for (int i = 0; i < 5; i++)
{
sensors = analogRead(i);
- // Round readings less than one hundred down to zero to filter*
- // out surface noise*
_ if (sensors < 100)_
* {*
_ sensors = 0;
* }
}
}
//********************************************************************_
// GET_AVERAGE FUNCTION GET_AVERAGE FUNCTION GET_AVERAGE FUNCTION GET_
//********************************************************************
void get_average()
_{_
// Zero the sensors_average variable*
* sensors_average = 0;*
* // Generate the weighted sensor average starting at sensor one to*
* // keep from doing a needless multiplicaiton by zero which will*
* // always discarded any value read by sensor zero*
* for (int i = 1; i < 5; i ++)*
* {*
sensors_average += sensors * i * 1000;
* }*
}
//********************************************************************
// GET_SUM FUNCTION GET_SUM FUNCTION GET_SUM FUNCTION GET_SUM FUNCTION
//********************************************************************
void get_sum()
{
* // Zero the sensors_sum variable*
* sensors_sum = 0;*
* // Sum the total of all the sensor readings*
* for (int i = 0; i < 5; i++)*
* {*
sensors_sum += int(sensors*);
_ }
}
//********************************************************************_
// GET_POSITION FUNCTION GET_POSITION FUNCTION GET_POSITION FUNCTION G*
//********************************************************************
void get_position()
{
* // Calculate the current position on the line*
* position = int(sensors_average / sensors_sum);
_}
//********************************************************************_
// GET_PROPORTIONAL FUNCTION GET_PROPORTIONAL FUNCTION GET_PROPORTIONA*
//********************************************************************
void get_proportional()
{
* // Caculate the proportional value*
* proportional = position - 2000;*
}
//********************************************************************
// GET_INTEGRAL FUNCTION GET_INTEGRAL FUNCTION GET_INTEGRAL FUNCTION G
//********************************************************************
void get_integral()
{
* // Calculate the integral value*
* integral = integral + proportional;*
}
//********************************************************************
// GET_DERIVATIVE FUNCTION GET_DERIVATIVE FUNCTION GET_DERIVATIVE FUNC
//********************************************************************
void get_derivative()
{
* // Calculate the derivative value*
* derivative = proportional - last_proportional;*
* // Store proportional value in last_proportional for the derivative*
* // calculation on next loop*
* last_proportional = proportional;
_}
//********************************************************************_
// GET_CONTROL FUNCTION GET_CONTROL FUNCTION GET_CONTROL FUNCTION GET_
//********************************************************************
void get_control()
_{
// Calculate the control value*
control_value = int(proportional * Kp + integral * Ki + derivative * Kd);
}
//
// ADJUST_CONTROL FUNCTION ADJUST_CONTROL FUNCTION ADJUST_CONTROL FUNC
//
void adjust_control()
{
* // If the control value is greater than the allowed maximum set it*
* // to the maximum*
* if (control_value > max_difference)*
* {_
control_value = max_difference;
_ }*_
* // If the control value is less than the allowed minimum set it to*
* // the 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 is less than zero a right turn is needed to*
* // acquire the center of the line*
* if (control_value < 0)
_ {_
analogWrite(RIGHT_MOTOR, max_difference + control_value);
analogWrite(LEFT_MOTOR, max_difference);
_ }_
// If control_value is greater than zero a left turn is needed to*
* // acquire the center of the line*
* else*
* {*
* analogWrite(RIGHT_MOTOR, max_difference);
analogWrite(LEFT_MOTOR, max_difference - control_value);
_ }
}[/size]*_