Arduino UNO - PID control Pololu DC motor with uadrature encoder

Other Hardware Motors, Mechanics, Power and CNC
1

Hallo everyone,

I'm trying to run the DC motor with encoder quadrature. I use:
1.Arduino Uno
2. Motor Driver: Quad_Motor_Driver_Shield_for_Arduino_SKU_DRI0039-DFRobot
3. 4.4:1 Metal Gearmotor 25Dx48L mm LP 12V with 48 CPR Encoder Pololu - 4.4:1 Metal Gearmotor 25Dx60L mm LP 12V with 48 CPR Encoder (No End Cap)

This is my code:

boolean A,B;

//Input PWM
const int STEP = 11;
//Input Direction
const int DIR = 12;

byte state, statep;

float setpoint = 100; //Set velocity

double Kp = 0.5;
double Ki = 0.8;
double Kd = 0;


int PWM_val = 0;                                // (25% = 64; 50% = 127; 75% = 191; 100% = 255)
int speed_act = 0;
float motorSpeed = 0;
float motorSpeed_float = 0;


float count = 0; //set the counts of the encoder

unsigned long lastMilliPrint = 0;           // loop timing
unsigned long lastMilli = 0;               // loop timing
unsigned long lastMilliEncoder = 0;        // loop timing


void setup() {
  Serial.begin(9600);
  
  pinMode(2, INPUT_PULLUP);//encoder A pin
  pinMode(3, INPUT_PULLUP);//encoder B pin
  
  attachInterrupt(0,Achange,CHANGE);//interrupt pins for encoder
  attachInterrupt(1,Achange,CHANGE); 
  
  pinMode(STEP, OUTPUT);
  pinMode(DIR, OUTPUT);
}

void loop(){                 

 if((millis()-lastMilliEncoder) >= 1000)   { 
     lastMilliEncoder = millis();
     getPosition(); 
  
 }
 if((millis()-lastMilliPrint) >= 500)   { 
  lastMilliPrint = millis();
    Serial.print("SP:");             Serial.print(setpoint );  
   Serial.print("  RPM:");          Serial.print(speed_act);
   Serial.print("  PWM:");          Serial.print(PWM_val);     
   Serial.print("\n");   
 }

 if((millis()-lastMilli) >= 100)   {  
     lastMilli = millis();                  
     PWM_val= updatePid(PWM_val, setpoint, speed_act);         // compute PWM value
     analogWrite(STEP, PWM_val);                                // send PWM to motor    
 }  
}


void getPosition()
{
static float countAnt = 0;                                                   // last count
 motorSpeed_float = ((count - countAnt)*60.0/211.2);          // 48 pulses X 4.4 gear ratio = 211.2 counts per one rotate
 speed_act = motorSpeed_float;
 countAnt = count; 
} 
   

int updatePid(int command, int targetValue, int currentValue)   {             // compute PWM value
float pidTerm = 0;                                                            // PID correction
int error=0;                                  
static int last_error=0;                            
 error = abs(targetValue) - abs(currentValue);
 pidTerm = (Kp * error) + (Kd * (error - last_error));                            
 last_error = error;
 return constrain(command + int(pidTerm), 0, 255);
}

  
void Achange() //these functions are for finding the encoder counts
{
  A = digitalRead(2);
  B = digitalRead(3);

  if ((A==HIGH)&&(B==HIGH)) state = 1;
  if ((A==HIGH)&&(B==LOW)) state = 2;
  if ((A==LOW)&&(B==LOW)) state = 3;
  if((A==LOW)&&(B==HIGH)) state = 4;
  switch (state)
  {
    case 1:
    {
      if (statep == 2) count++;
      if (statep == 4) count--;
      break;
    }
    case 2:
    {
      if (statep == 1) count--;
      if (statep == 3) count++;
      break;
    }
    case 3:
    {
      if (statep == 2) count --;
      if (statep == 4) count ++;
      break;
    }
    default:
    {
      if (statep == 1) count++;
      if (statep == 3) count--;
    }
  }
  statep = state;

}

It seems to me that the reading from the encoder is correct. However, now it works so that the engine rotates for a second and then stands for a second and then in a circle.

2

You only update the position and speed once per second but you are trying to update the PWM much more frequently. How can PWM change when there is no new data?

Your speed calculation needs to be overhauled so that it can be called with different intervals.

3

Thanks for the answer. I wanted to take measurements from the encoder within 1 second to increase the accuracy of the speed measurement.

4

You can update position and speed frequently but average the speed over a longer period. For example, store the last 3 positions and the millis() for each of those. Then you can measure the current position relative to 3 time-units ago to calculate speed, then store the latest position in that space.

Note that this kind of averaging will make your PID control more difficult because it is reacting to part of an old speed that it has already tried to adjust for.