Arduino - PD - Line Following - arduino

Question

#include <QTRSensors.h>
#define NUM_SENSORS 5 //Number of sensors used
#define NUM_SAMPLES_PER_SENSOR 4 //Average 4 analog samples per sensor reading
#define EMITTER_PIN 13 //Emitter is controlled by digital pin 2
QTRSensorsAnalog qtra((unsigned char[]) {0,1,2,3,4},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS]; //Variable Array for sensor values
const double kp = .505; //Variable for adjusting KP Value
const double kd = 150; //Variable for adjusting KD Value
const int max_speed = 255; //Variable for the Maximum Speed of the Motors
unsigned int position = 0; //Variable for holding the calculated position
int set_point = 1500;//Variable for holding the value of the center of the line
int error = 0; //Variable for holding the error from center of line based on position
int last_error = 0; //Variable for holding the previous error
int white = 0; //Variable for reading the line over white or black
int max_difference = 80 ;
double spd_right; //Speed for the Right Motor
double spd_left; //Speed for the Left Motor
int derivative = 0; //Value for the derivative
int error_value = 0; //Value for the error_value calculated from the pd function
int key_s6 = 2;//Declaring Digital Push Button
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M2 Direction Control
int sensors_sum = 0;
//Setup Method that includes calibration
void setup(){
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(13, OUTPUT);
calibration();
set_Point();
}//Ends Setup
void loop (){
get_sum();
white_or_black();
pid_calc();
adjust_control();
set_motors();
}//Ends Loop*/
/*Method for calibrating set_point on individual tracks
*/
void set_Point(){
qtra.readCalibrated(sensorValues);
for (int i = 0; i <= 5; i++){
set_point = sensorValues[i] + set_point;
}//Ends For
}//Ends set_point Method
/*Method for Calibrating the sensors to the ambiance lighting and
* varying contrasts of each individual track.
*/
void calibration (){
for (int i = 0; i<250; i++){
qtra.calibrate();
delay(20);
}//Ends For
}//Ends Calibration Method
/*Method for calculating the sum of all the sensors to be used in the
* white_or_black method for determining the background color of the track
*/
void get_sum(){
qtra.readCalibrated(sensorValues);
for (int i = 0; i <= 5; i++){
sensors_sum = sensorValues[i] + sensors_sum;
}//Ends For
}//Ends get_sum Method
/*Method for determining the background color of the current portion of the
* track based off of the value returned by get_sum and adjusting a variable
* to follow a white of black line as necessary.
*/
void white_or_black(){
if (sensors_sum > 2000){
white = 1;
}//Ends If
else{
white = 0;
}//Ends Else
}//Ends white_or_black Method
/*Method for following the line based of the position, proportional,
* and derivative
*/
void pid_calc(){
position = qtra.readLine(sensorValues, QTR_EMITTERS_ON, white);
error = position - set_point;
derivative = error-last_error;
last_error = error;
error_value = int(error*kp+derivative*kd);
}//Ends pid_calc Method
void adjust_control(){
if (error_value>max_difference){
error_value = max_difference;
}//Ends If
else if(error_value < max_difference){
error_value = -max_difference;
}//Ends If
if (error_value < 0 ){
spd_right = max_speed + error_value;
spd_left = max_speed;
}//Ends If
else{
spd_right = max_speed;
spd_left = max_speed - error_value;
}//Ends Else
}//Ends adjust_control Method
void set_motors(){
analogWrite(M1, HIGH);
analogWrite(M2, HIGH);
analogWrite(E1, constrain(spd_right, 0, max_speed));
analogWrite(E2, constrain(spd_left, 0, max_speed));
}//Ends set_motors Method
void stop(){
analogWrite(E1, 0);
analogWrite(E2, 0);
}//Ends Stop Method
I'm new to the arduino language. This is my first program and I wrote it last night. It's a PD Line Following example.
I'm currently having issues with the motors adjusting. Simply put it just goes straight.
I'm using a QTR Analog Sensor Array. I might switch to digital because one of the analog inputs on the romeo board I have is not working. Does anyone one have any suggestions or improvements to my logic?

Answer 1

void set_Point(){
qtra.readCalibrated(sensorValues);
for (int i = 0; i < NUM_SENSORS; i++){
set_point += sensorValues[i];
}
}
A strange thing about this loop is that is set_point is not initialised (to zero) before adding, and not divided (by i==NUM_SENSORS-=5) after the summation.