Trouble with PID

VEX V5 Technical Support
#1

hello and good day everyone,
i want to create a PID program for Line following Robot and i am very new with it, the code seems does not work well. here’s my coding if someone could help me with it, thanks.

#pragma config(Sensor, port7, S5, sensorVexIQ_ColorGrayscale)
#pragma config(Sensor, port8, S4, sensorVexIQ_ColorGrayscale)
#pragma config(Sensor, port9, S3, sensorVexIQ_ColorGrayscale)
#pragma config(Sensor, port10, S2, sensorVexIQ_ColorGrayscale)
#pragma config(Sensor, port11, S1, sensorVexIQ_ColorGrayscale)
#pragma config(Sensor, port12, T, sensorVexIQ_LED)
#pragma config(Motor, motor1, R, tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor, motor6, L, tmotorVexIQ, PIDControl, encoder)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//

task main()
{
int SP= 200;

int init_speed = 50;
		

float kp =0.038,
			kd =0.26;

float error,
			lastError,
			deravative,
			PD;

while(1==1)

{
	displaySensorValues(line1, S1);
displaySensorValues(line2, S2);
displaySensorValues(line3, S3);
displaySensorValues(line4, S4);
displaySensorValues(line5, S5);

	if(SensorValue[S3]>SP)
	{
		setTouchLEDRGB(T, 0, 255, 0);
		error = SP - SensorValue[S3]*(0);
	}
	else if(SensorValue[S1]>SP)
	{
		setTouchLEDRGB(T, 255, 0, 0);
		error = SP -(2.5)*SensorValue[S1];
	}
	else if(SensorValue[S2]>SP)
	{
		setTouchLEDRGB(T, 0, 0, 255);
		error = SP - (2)*SensorValue[S2];
	}
	else if(SensorValue[S4]>SP)
	{
		setTouchLEDRGB(T, 255, 255, 0);
		error = SP - (-2)*SensorValue[S4];
	}
	else if(SensorValue[S5]>SP)
	{
		setTouchLEDRGB(T, 0, 255, 255);
		error = SP - (-2.5)*SensorValue[S5];
	}

	else if(SensorValue[S3]>SP && SensorValue[S4]>SP)
	{
		setTouchLEDRGB(T, 200, 200, 200);
		error = SP - ((SensorValue[S3]*(0)+SensorValue[S4]*(-2)/2));
	}
	else if(SensorValue[S3]>SP && SensorValue[S2]>SP)
	{
		setTouchLEDRGB(T, 200, 200, 200);
		error = SP - ((SensorValue[S3]*(0)+SensorValue[S4]*(2)/2));
	}

	else if(SensorValue[S3]>SP && SensorValue[S4]>SP && SensorValue[S5]>SP)
	{
		setTouchLEDRGB(T, 200, 200, 200);
		error = SP - ((SensorValue[S3]*(0)+SensorValue[S4]*(-2)+SensorValue[S5]*(-2.5))/3);
	}
	
	else if(SensorValue[S3]>SP && SensorValue[S2]>SP && SensorValue[S1]>SP)
	{
		setTouchLEDRGB(T, 200, 200, 200);
		error = SP - ((SensorValue[S3]*(0)+SensorValue[S2]*(2)+SensorValue[S1]*(2.5))/3);
	}
	
	
	else if(SensorValue[S2]>SP && SensorValue[S3]>SP && SensorValue[S4]>SP)
	{
		setTouchLEDRGB(T, 50, 50, 50);
		error = SP - ((SensorValue[S3]*(2)+SensorValue[S2]*(2)+SensorValue[S4]*(2))/3);
	}

	if(error!=0)
	{
		deravative = error-lastError;
		PD = kp*error+kd*deravative;
		lastError=error;
		motor[R] = init_speedR-PD;
		motor[L] = init_speedL+PD;
	}

	else
	{
		deravative = 0;
	}
}

}