1
\$\begingroup\$

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
\$\endgroup\$
  • 1
    \$\begingroup\$ I dont have experience in arduino but i saw that rWheel doest take overflow into account. Does millis() take overflow into account ? PID seems ok. I see you have anti-wind up implemented. Did you try PI only ? \$\endgroup\$ – Gossamer Mar 17 '13 at 21:41
  • \$\begingroup\$ millis() wont overflow over the small timescale that the motor is running on so I'm going going to worry about that. I did try PI only and adjusted the constraint I had on the integral term and it's working now. Thanks! If you submit as an answer I'll mark it correct. \$\endgroup\$ – JDD Mar 19 '13 at 22:22
1
\$\begingroup\$

I saw that rWheel doest take overflow into account. Does millis() take overflow into account ? PID seems ok. I see you have anti-wind up implemented. Did you try PI only ?

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.