Closed loop DC motor velocity control

I'm trying to set up a closed loop control for the velocity of a DC motor. I have a small DC motor with an 8bit encoder. I'm driving the motor using PWM off an arduino and a L298N h-bridge.

Here's the code I came up with. I had this working just fine for doing position control of the motor and now I'm trying to adapt it for velocity.

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++;
}

updatePID function

#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);
}

Here's some output showing the calculated wheel velocity Vr and the PWM setting calculated from the PID regulator. The target velocity is 150 mm/s for this example. I've tried playing with the PID gains but it doesn't seem to help. I feel like I'm missing some obvious thing here.

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