I use the PID algorithm to control the RPM of a micro Gearmotor. My motor and encoder specifications are given below.

Motor: https://www.pololu.com/product/2378 - 6V 29.86:1 gear ratio - please kindly note that I'm using a 9V battery. Encoder: https://www.pololu.com/product/4760 - 12 counts per revolution of the motor shaft

Also, using Arduino Mega MCU, and this is my sketch:

#include <util/atomic.h> // For the ATOMIC_BLOCK macro

#define ENCA 3 // YELLOW
#define ENCB 2 // WHITE
#define PWM 5
#define IN2 6
#define IN1 7

volatile int pos_i = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevT = 0;
int posPrev = 0; 

volatile float velocity_i = 0;
volatile long prevT_i = 0;

float v1Filt = 0;
float v1Prev = 0;
float v2Filt = 0;
float v2Prev = 0;

float eintegral = 0;
float eprev = 0;

void setup() {


void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  if(dir == 1){
  else if(dir == -1){

void loop() {

  int pos = 0;
  float velocity2 = 0;
    pos = pos_i;
    velocity2 = velocity_i;

  long currT = micros();
  float deltaT = ((float) (currT-prevT))/1.0e6;
  float velocity1 = (pos - posPrev)/deltaT;
  posPrev = pos;
  prevT = currT;

  float v1 = velocity1 / 89.58  * 60.0;
  float v2 = velocity2 / 89.58  * 60.0;

  // Low-pass filter (25 Hz cutoff)
  v1Filt = 0.854*v1Filt + 0.0728*v1 + 0.0728*v1Prev;
  v1Prev = v1;
  v2Filt = 0.854*v2Filt + 0.0728*v2 + 0.0728*v2Prev;
  v2Prev = v2;

  float vt = 60;

  float kp = 2.0;
  float ki = 4.5;
  float kd = 0.0;
  float e = vt - v1Filt;
  eintegral = eintegral + e*deltaT;

  float dedt = (e-eprev)/(deltaT);
  float u = kp*e + ki*eintegral;

  // Set the motor speed and direction
  int dir = 1;
  if (u<0){
    dir = -1;
  int pwr = (int) fabs(u);
  if(pwr > 255){
    pwr = 255;
  eprev = e;
  Serial.print(" ");

void readEncoder(){
  // Read encoder B when ENCA rises
  int b = digitalRead(ENCB);
  int increment = 0;
    // If B is high, increment forward
    increment = 1;
    // Otherwise, increment backward
    increment = -1;
  pos_i = pos_i + increment;

  // Compute velocity with method 2
  long currT = micros();
  float deltaT = ((float) (currT - prevT_i))/1.0e6;
  velocity_i = increment/deltaT;
  prevT_i = currT;

However, the motor RPM is rapidly oscillating, and it is not reaching the desired RPM level. I tried several Kp and Ki values, but still no luck.

Any help regarding this issue is highly appreciated.

Best regards, Upul

  • 1
    $\begingroup$ I hope your 9V battery is not the kind with snap connectors on top. Although the voltage seems generous, such batteries cannot supply enough current to operate your motor. Consider using four AA cells until you have debugged the software/controls. $\endgroup$
    – r-bryan
    May 2, 2022 at 17:59

1 Answer 1


An alternative to PID control is a constant step controller. It's a controller that looks at the error and then just adds (or subtracts) a constant amount (usually very small). It's very easy to implement and very intuitive. However, it is slow to reach the desired velocity. This can be mitigated with a feed forward mechanism; when starting from 'stopped' calculate the initial control value (throttle pwm value) based on measured values for stall throttle (the throttle (pwm value) below which the motor will not turn; so this also corresponds to the minimum possible non-zero velocity). Given the target velocity, you can do a linear interpolation between min and max velocity to estimate the feed forward value for throttle. Then the constant step controller will converge quickly from a standing start. If there is a large perturbation while moving it will still respond slowly.

This differential drive robot uses a feed forward constant step controller for speed control and a constant step controller for heading control during goal to goal behavior. https://github.com/Ezward/Esp32CameraRover2 Speed control is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/wheel/drive_wheel.cpp#L323, kinematic pose estimation is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/rover/rover.cpp#L313, and heading controller is here https://github.com/Ezward/Esp32CameraRover2/blob/3cb95bc326873d1e354887132699924332c0c7e8/src/rover/goto_goal.cpp#L333 Here an example of it in operation https://youtu.be/TjE9ceNOTJE


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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