const int motor3Pin = 3;

const int motor1Pin = 10;

const int motor2Pin = 6;

const int motor4Pin = 11;

int right_speed = 0;

int left_speed = 0;

long sensors[] = {0,0,0,0,0};

long sensors_average = 0;

int sensors_sum = 0;

int posn; //position

int proportional = 0;

long integral = 0;

int derivative = 0;

int last_proportional = 0;

float Kp = 2; // Havent decided the values yet

float Ki = 2;

float Kd = 2;

int error_value = 0;

int max_speed = 200;

int set_point = 1987;//the posn on the center of line, I have to decide the value yet

void setup(){

pinMode(motor1Pin, OUTPUT);

pinMode(motor2Pin, OUTPUT);

pinMode(motor3Pin, OUTPUT);

pinMode(motor4Pin, OUTPUT);

Serial.begin(9600);

}

void read_sensors () {

for (int i = 0; i < 5; i++) {

sensors* = analogRead(i);*

sensors_average += sensors* * i * 1000;*

sensors_sum += int(sensors*);*

}

}

void pid_calc () {

posn = int(sensors_average / sensors_sum);

proportional = posn - set_point;

integral = integral + proportional;

derivative = proportional - last_proportional;

last_proportional = proportional;

error_value = int(proportional * Kp + integral * Ki + derivative * Kd);

}

void calc_turn(){

if (error_value < -256) {

error_value = -256;

}

if (error_value > 256) {

error_value = 256;

}

if (error_value < 0) {

right_speed = max_speed + error_value;

left_speed = max_speed;

}

else {

right_speed = max_speed;

left_speed = max_speed + error_value;

}

}

void motor_drive (int right_speed, int left_speed) {

analogWrite(motor1Pin, right_speed);

analogWrite(motor2Pin, left_speed);

delay(20);

}

void loop () {

read_sensors ();

pid_calc ();

calc_turn ();

motor_drive (right_speed, left_speed);

Serial.print(error_value);

Serial.print(" ");

Serial.print(right_speed);

Serial.print(" ");

Serial.print(left_speed);

Serial.println(" ");

}