#pragma config(Sensor, in1,    LINE1,          sensorLineFollower)
#pragma config(Sensor, in2,    LINE2,          sensorLineFollower)
#pragma config(Sensor, in3,    LINE3,          sensorLineFollower)
#pragma config(Sensor, in4,    COLOR1,         sensorLineFollower)
#pragma config(Sensor, in5,    COLOR2,         sensorLineFollower)
#pragma config(Motor,  port2,           rightMOTOR,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           leftMOTOR,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           VER,           tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port5,           HOR,           tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int mySpeed=25;
int IRSensorValues[3]={0,0,0};
int sum=0;
int difference = 0;
int sumR=0;
int sumL=0;
int RL =0;
// PID  var
float kp = .8;
float  kd=0.15;
float setpoint =0;
float  Error=0;
float LastError=0;
float P_PID=0;
float D_PID=0;
float PD=0;
float LoopTime=3;
float PIDError=0;
float PIDDerivative = 0;
float PIDLastError =0;
float PIDDrive = 0;
//float  pidIntegral;


void ReadSensors()

{
	IRSensorValues[0]=SensorValue[LINE1]*(255.0/4096.0)*63/104;
	//	wait1Msec(20);
	IRSensorValues[1]=SensorValue[LINE2]*(255.0/4096.0);
	if(IRSensorValues[1]>110){IRSensorValues[1]=110;}
	//wait1Msec(20);
	IRSensorValues[2]=SensorValue[LINE3]*(255.0/4096.0)-68;
	//	wait1Msec(20);
	sum=IRSensorValues[0]+IRSensorValues[1];
	difference=(IRSensorValues[0]-IRSensorValues[2]);
	sumR=IRSensorValues[1]+IRSensorValues[0];
	sumL=IRSensorValues[2]+IRSensorValues[1];
	RL=sumR-sumL;
}
int pidcontroller()
{
	Error=setpoint+RL;
	P_PID=Error*kp;
	D_PID=((Error-LastError)/(LoopTime/1000))*kd;
	PD=P_PID+D_PID;
	LastError=Error;
	if(PD>mySpeed){PD=mySpeed;}
	return PD;
}
/*
    while(true)
    	{
    			PIDError = RL-setpoint;
    			//calc derivative
    			PIDDerivative = PIDError - PIDLastError;
          PIDLastError  = PIDError;
          PIDDrive = ((RL*kp)+(PIDDerivative*kd));

  		}*/


void MotorControl(int PowerInitial)
{
	int Motorpower1=(PowerInitial-(PIDDrive));
	int Motorpower2=(PowerInitial+(PIDDrive));

	if(Motorpower1>127){
		Motorpower1 = 127;
	}
		else if(Motorpower1<-127){
		Motorpower1 = -127;
	}
		if(Motorpower2>127){
		Motorpower2 = 127;
	}
		else if(Motorpower2<-127){
		Motorpower2 = -127;
	}

	motor[rightMOTOR]=Motorpower1;
	motor[leftMOTOR]=Motorpower2;
}

void MotorControlPID(int PowerInitial)
{
	int Motorpower1=(PowerInitial+pidcontroller());
	int Motorpower2=(PowerInitial-pidcontroller());

	if(Motorpower1>127){
		Motorpower1 = 127;
	}
		else if(Motorpower1<-127){
		Motorpower1 = -127;
	}
		if(Motorpower2>127){
		Motorpower2 = 127;
	}
		else if(Motorpower2<-127){
		Motorpower2 = -127;
	}

motor[rightMOTOR]=Motorpower1;
motor[leftMOTOR]=Motorpower2;

}


task main()
{

	while(true)
	{
		time1[T1]=0;
		ReadSensors();
	  MotorControlPID(mySpeed);
    while(time1[T1]<LoopTime){}
	}
}