Arduino robot control PID praćenje linije

Pozz, zanima me zna li itko kako da mi robot prati liniju imam algoritam po kojemu bi on trebao pratiti liniju međutim kada robot pokrenem i stavim na liniju motori za kotače mi ne radi.

Dok kad stavim na IR senzor neki objekt crne boje IR senzor mi se pali što i treba raditi. Evo kod pa ako netko može pogledati i savjetovati do čega je problem, hvala.

float Kp=1,Ki=0.2,Kd=0;
float error=0, P=0, I=0, D=0, PID_value=0;
float previous_error=0, previous_I=0;
int sensor[5]={0, 0, 0, 0, 0};
int initial_motor_speed=100;

void read_sensor_values(void);
void calculate_pid(void);
void motor_control(void);

void setup()
{
 pinMode(9,OUTPUT); //PWM Pin 1
 pinMode(10,OUTPUT); //PWM Pin 2
 pinMode(4,OUTPUT); //Left Motor Pin 1
 pinMode(5,OUTPUT); //Left Motor Pin 2
 pinMode(6,OUTPUT); //Right Motor Pin 1
 pinMode(7,OUTPUT);  //Right Motor Pin 2
 Robot.begin();
 Serial.begin(9600); //Enable Serial Communications
 
}

void loop()
{
    read_sensor_values();
    calculate_pid();
    motor_control();
}

void read_sensor_values()
{
  sensor[0]=digitalRead(A0);
  sensor[1]=digitalRead(A1);
  sensor[2]=digitalRead(A2);
  sensor[3]=digitalRead(A3);
  sensor[4]=digitalRead(A4);
 
  if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==1))
  error=4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==1)&&(sensor[4]==1))
  error=3;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==1)&&(sensor[4]==0))
  error=2;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[4]==1)&&(sensor[4]==0))
  error=1;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[4]==0)&&(sensor[4]==0))
  error=0;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-1;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-2;
  else if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-3;
  else if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
  error=-4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==0))
    if(error==-4) error=-5;
    else error=5;

}

void calculate_pid()
{
    P = error;
    I = I + previous_I;
    D = error-previous_error;
   
    PID_value = (Kp*P) + (Ki*I) + (Kd*D);
   
    previous_I=I;
    previous_error=error;
}

void motor_control()
{
    // Calculating the effective motor speed:
    int left_motor_speed = initial_motor_speed-PID_value;
    int right_motor_speed = initial_motor_speed+PID_value;
   
    // The motor speed should not exceed the max PWM value
    constrain(left_motor_speed,0,255);
    constrain(right_motor_speed,0,255);
   
    Serial.println(left_motor_speed);
    Serial.println(right_motor_speed);  
   
    analogWrite(9,initial_motor_speed-PID_value);   //Left Motor Speed
    analogWrite(10,initial_motor_speed+PID_value);  //Right Motor Speed
    //following lines of code are to make the bot move forward
    /*The pin numbers and high, low values might be different
    depending on your connections */
    digitalWrite(4,HIGH);
    digitalWrite(5,LOW);
    digitalWrite(6,LOW);
    digitalWrite(7,HIGH);
}

Evo testirao sam kod i radi mi na robotu, drugačiji je u odnosu na ovaj gore, međutim kada dodam kod za pokretanje motora robot mi poludi i daje krive rezultate za očitanja senzora sa serial port-a.

#include <ArduinoRobot.h>

int sensor[5]={0, 0, 0, 0, 0};
float Kp=1,Ki=0,Kd=0.5;
float error=0, P=0, I=0, D=0, PID_value=0;
float previous_error=0, previous_I=0;

void setup(){
// initialize the robot
Robot.begin();

// initialize the serial port
Serial.begin(9600);
}

void loop(){
// store the sensor information into the array
Robot.updateIR();

// iterate the array and print the data to the Serial port
for(int i=0; i<5; i++){
// Serial.print(Robot.IRarray[i]); // izbacivalo je 960 960 960 960 320
// Serial.print(" ");

sensor[i] = Robot.IRarray[i]; // spremam ovo odmah u IRDigSensorArray[i] svih pet vrijednosti
if(sensor[i] >= 0 && sensor[i] <= 350) // pretvorba u binarni sustav
{
sensor[i] = 1;
Serial.print(sensor[i]);
Serial.print(" ");
}

else
{
sensor[i] = 0;
Serial.print(sensor[i]);
Serial.print(" ");
}


}
read_sensor_values();
calculate_pid();

//Serial.print(error);
Serial.println("");
}

void read_sensor_values()
{

if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==1))
error=4;
else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==1))
error=3;
else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==0))
error=2;
else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==1)&&(sensor[4]==0))
error=1;
else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0))
error=0;
else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0))
error=-1;
else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
error=-2;
else if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
error=-3;
else if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
error=-4;
else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
if(error==-4) error=-5;
else error=5;

}

void calculate_pid()
{
P = error;
I = I + previous_I;
D = error-previous_error;

PID_value = (Kp*P) + (Ki*I) + (Kd*D);

previous_I=I;
previous_error=error;

Serial.print(PID_value);
}