control de velocidad del motor de CC en bucle cerrado

Estoy intentando configurar un control de bucle cerrado para la velocidad de un motor de CC. Tengo un pequeño motor de corriente continua con un codificador de 8 bits. Estoy manejando el motor usando PWM en un arduino y un L298N h-bridge.

Aquí está el código que se me ocurrió. Hice que funcionara bien para hacer el control de posición del motor y ahora estoy tratando de adaptarlo a la velocidad.

int ticks = 0; // encoder ticks
int ticks2 = 0; // stored copy of interrupt ticks
int error, drive;
int pinI1=8; // enable left wheel motor
int pinI2=11; // enable left wheel motor
int pinI3=12; // enable right wheel motor
int pinI4=13; // enable right wheel motor
int speedpinA=9;//left motor
int speedpinB=10;//right motor

double tStart=0, loopTime=0, tStop;
float  Vr, Vl; //wheel velocities

//********************Change the tuning parameters here**********************
//Velocity set point.  Number of encoder ticks.  256/rev. 
int velocity = 150; //  mm/s
//How close to the desired setpoint before we switch to the conservative PID
int gapDist=1;
//Aggressive
double aggK=1, aggKp=1, aggKi=2, aggKd=0.2;
//Conservative
double consK=1, consKp=1, consKi=2, consKd=0.2;
//***************************************************************************


void setup(){
  Serial.begin(9600);
  attachInterrupt(0, rWheel, RISING);
  pinMode(pinI1,OUTPUT);
  pinMode(pinI2,OUTPUT);
  pinMode(speedpinA,OUTPUT);
  pinMode(pinI3,OUTPUT);
  pinMode(pinI4,OUTPUT);
  pinMode(speedpinB,OUTPUT);
}


void forward(){
     analogWrite(speedpinA,drive);
     analogWrite(speedpinB,drive);
     digitalWrite(pinI4,LOW);//right wheel forward
     digitalWrite(pinI3,HIGH);
     digitalWrite(pinI2,LOW);//right wheel forward
     digitalWrite(pinI1,HIGH);
   }

void backward(){
     analogWrite(speedpinA,drive);//input a simulation value to set the speed
     analogWrite(speedpinB,drive);
     digitalWrite(pinI4,HIGH);//right wheel backward
     digitalWrite(pinI3,LOW);
     digitalWrite(pinI2,HIGH);//left wheel backward
     digitalWrite(pinI1,LOW);
   } 

void Stop(){
     digitalWrite(speedpinB, LOW);  
     ticks = 0; 
     delay(1000);
   }



void loop(){
  ticks2 = ticks; //Store a copy of the interrupt ticks 

//******* Set up for PID **********************************************************
  double gap = abs(velocity - Vr); //distance away from setpoint
  if(gap <gapDist)
  { //we're close to setpoint, use conservative tuning parameters
    drive = updatePid(velocity,Vr, consK, consKp, consKi, consKd);
  }
  else
  {
     //we're far from setpoint, use aggressive tuning parameters
    drive = updatePid(velocity,Vr, aggK, aggKp, aggKi, aggKd);
  }
//********************************************************************************  

  // minimum PWM to drive the motors
   if(drive < 40 && drive > 0){ drive = 40;}
   if(drive > -40 && drive < 0){ drive = -40;}

   forward(); // drive forward 

 Vr = (0.8741/loopTime)*1000; // Calculate wheel velocity mm/s, 0.8741 mm/tick

 Serial.print("Vr: "); Serial.print(Vr); Serial.print(" "); Serial.print("PWM: "); Serial.println(drive);

}

// wheel encoder interrupt
void rWheel(){
  loopTime = millis() - tStart;
  tStart = millis();
  ticks = ticks++;
}

función updatePID

#define GUARD_GAIN 20.0 
int last_error = 0;
int integrated_error = 0;
int pTerm = 0, iTerm = 0, dTerm = 0;

int updatePid(int targetPosition, int currentPosition, float K, int Kp, int Ki, int Kd) {
  error = targetPosition - currentPosition;
  pTerm = Kp * error;
  integrated_error += error;
  iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
  dTerm = Kd * (error - last_error);
  last_error = error;
  return constrain(K*(pTerm + iTerm + dTerm), 0, 255);
}

Aquí hay una salida que muestra la velocidad de rueda calculada Vr y la configuración PWM calculada desde el regulador PID. La velocidad objetivo es de 150 mm / s para este ejemplo. He intentado jugar con las ganancias PID pero no parece ayudar. Siento que me estoy perdiendo algo obvio aquí.

Vr: 145.68 PWM: 190
Vr: 145.68 PWM: 45
Vr: 174.82 PWM: 45
Vr: 124.87 PWM: 40
Vr: 145.68 PWM: 66
Vr: 145.68 PWM: 45
Vr: 109.26 PWM: 45
Vr: 109.26 PWM: 81
Vr: 97.12 PWM: 81
Vr: 97.12 PWM: 93
Vr: 124.87 PWM: 93
Vr: 124.87 PWM: 66
Vr: 109.26 PWM: 66
Vr: 124.87 PWM: 81
Vr: 124.87 PWM: 66
Vr: 109.26 PWM: 66
Vr: 109.26 PWM: 81
Vr: 109.26 PWM: 81
Vr: 109.26 PWM: 81
Vr: 124.87 PWM: 81
Vr: 124.87 PWM: 66
Vr: 109.26 PWM: 66
Vr: 109.26 PWM: 81
Vr: 109.26 PWM: 81
Vr: 109.26 PWM: 81
Vr: 109.26 PWM: 81
Vr: 124.87 PWM: 81
Vr: 124.87 PWM: 66
Vr: 109.26 PWM: 66
Vr: 109.26 PWM: 81