Seguidor de linea con Encoder

Saludos miembros del grupo estoy implementando un robot velocista con encoder, el objetivo es leer la velocidad real para poder acelerar en rectas y curvas pero me he leído varios tutoriales y no logro entender bien el tema.
HE IMPLEMENTADO DOS PID
UNOS PARA EL CONTROL DE VELOCIDAD, EL OTRO PARA LA VELOCIDAD ANGULAR

BUENO LES DEJO FOTOS DEL ROBOT Y LA PROGRAMACIÓN
/////////////sensores//////////////////
#include <QTRSensors.h>

#define STBY 7
#define BIN1 4
#define BIN2 5
#define PWMA 6
#define PWMB 3
#define AIN1 9
#define AIN2 8
#define NUM_SENSORS 6
#define TIMEOUT 4 // tiempo de espera para dar resultado en uS
#define LED 13
/////////////////Constante PD//////////////////
float Kp1 = 0.12;
float Kd1 = 2;
float Velmax=0;
// Data para intrgal
int error1 = 0;
int error2 = 0;
int error3 = 0;
int error4 = 0;
int error5 = 0;
int error6 = 0;
// Configuración de la librería QTR-8A
QTRSensorsAnalog qtra((unsigned char){A0, A1, A2, A3, A4, A5}
, NUM_SENSORS, TIMEOUT);

unsigned int sensorValues[NUM_SENSORS];

/////////////// variables de control velocidad//////////////////////////////////////////////
float Kp=2.8,Kd=5.5;//Ki=0.00
double Rproportional, Rlast_proportional=0; // error
double Rderivative;
double Pow; // Señal control
// constantes del controlador
float SETPOINT;
float velprom;
int resolucion=255;
/////////////////varibles de velocidad///////////
int encoder1=2;
float rpmizq=0;
float velizq=0;
volatile float pulsosizq=0;
unsigned long timeold = 0;
unsigned int pulsesperturn = 3;
unsigned int wheel_diameter = 24; // Diámetro de la rueda pequeña[mm]
int ratio = 10; //relacion
int sampletime=100;

//////////////////////////////////
// Función accionamiento motor izquierdo
void Motoriz(int value)
{
if ( value >= 0 )
{
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}
else
{
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
value *= -1;
}
analogWrite(PWMB, value);
}

// Función accionamiento motor derecho
void Motorde(int value)
{
if ( value >= 0 )
{
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
}
else
{
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
value *= -1;
}
analogWrite(PWMA, value);
}

//Accionamiento de motores
void Motor(int left, int righ)
{
digitalWrite(STBY, HIGH);
Motoriz(left);
Motorde(righ);
}

//función de freno
void freno(boolean left, boolean righ, int value)
{
digitalWrite(STBY, HIGH);
if ( left )
{
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, HIGH);
analogWrite (PWMB, value);
}
if ( righ )
{
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, HIGH);
analogWrite (PWMA, value);
}
}

////////////////////////////////
void counter_1(){
pulsosizq ++; }

void setup() {
Serial.begin(9600);
// Declaramos como salida los pines utilizados
pinMode(LED , OUTPUT);
pinMode(BIN2 , OUTPUT);
pinMode(STBY , OUTPUT);
pinMode(BIN1 , OUTPUT);
pinMode(PWMB , OUTPUT);
pinMode(AIN1 , OUTPUT);
pinMode(AIN2 , OUTPUT);
pinMode(PWMA , OUTPUT);
pinMode(encoder1, INPUT);

// Calibramos con la función qtra.calibrate();, y dejamos parpadeando el led, mientras se produce la calibración.
digitalWrite(LED, HIGH);
for ( int i = 0; i < 400; i++)
{
qtra.calibrate();
}
digitalWrite(LED, LOW);
// boton

attachInterrupt(0, counter_1, RISING);

}
///////////////pid de sensores
unsigned int position = 0;
//declaraos variables para utilizar PID
int proporcional = 0; // Proporcional
int derivativo = 0; // Derivativo
double diferencial = 0; // Diferencia aplicada a los motores
int last_prop; // Última valor del proporcional (utilizado para calcular la derivada del error)
int Target = 2500; // Setpoint (Como utilizamos 6 sensores, la línea debe estar entre 0 y 5000, por lo que el ideal es que esté en 2500)
/////////////////////////
int sensores(){
unsigned position = qtra.readLine(sensorValues);
proporcional = ((int)position) - 2500;

derivativo = proporcional - last_prop;
last_prop = proporcional;

error6 = error5;
error5 = error4;
error4 = error3;
error3 = error2;
error2 = error1;
error1 = proporcional;

diferencial = ( proporcional * Kp1 ) + ( derivativo * Kd1) ;

if ( diferencial > Velmax ) diferencial = Velmax;
else if ( diferencial < -Velmax ) diferencial = -Velmax;

return diferencial;
}
void loop() {
Velmax=50;
// lectura();
//Serial.print(" diferencial ");
// Serial.println(diferencial);
vel();
sensores();
controladorPID();
Motor(0, diferencial + Pow);
//EXPLICO UN POCO ESTA SEPARADA LA VELOCIDAD LINEAL( pow) Y ANGULAR (DIFERENCIAL)
//
//Serial.print("PV “); //PV
//Serial.print(velizq);
//Serial.print(” SP “); //sp
//Serial.print(Velmax);
//Serial.print(” "); //sp

// Motor(0,0);
//
delay(50);

}
void lectura(){

unsigned position = qtra.readLine(sensorValues);
//sesnores

for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
Serial.print(sensorValues*);*

  • Serial.print(’\t’);*
    }
    Serial.println(); // uncomment this line if you are using raw values
    Serial.println(position);
    delay(250);
    //--------------

  • }*
    void vel(){

  • if(millis()-timeold > 20){*

  • noInterrupts();*
    rpmizq= 60 * pulsosizq / pulsesperturn * 1000 / (millis() - timeold) ;
    velizq= rpmizq/ratio * 3.1416 * wheel_diameter * 60 / 1000000; //
    _ velizq=5*velizq/18;// transformar ha m/seg_

  • pulsosizq= 0; // Inicializamos los pulsos.*

  • timeold = millis(); // Almacenamos el tiempo actual.*

  • interrupts(); // Restart the interrupt processing // Reiniciamos la interrupción*

  • }*

  • }*
    int controladorPID() {
    _ Velmax=14.5*Velmax;_

  • Rproportional = Velmax - rpmizq;*

  • Rderivative = Rproportional - Rlast_proportional;*

  • Rlast_proportional = Rproportional;*
    _ Pow = Pow + (Rproportional * Kp) + (Rderivative * Kd);_

  • if (Pow > resolucion)*

  • Pow = resolucion;*

  • else if (Pow < -resolucion)*

  • Pow = -resolucion;*

  • return Pow;*

}