PID motor controller with load compensation.

Im wondering if the motor driver is a bit weak and just cant deliver the power to the motor that the PWM is demanding since it can only output 1.2A continuous. 

I have one of these that i have not tried. Can deliver 1.5A peak. Wondering if that could help? 

https://www.sparkfun.com/products/14450  

@anupamd Wiring diagrams?, sketch?. We aren't mind readers.

@anupamd 

From the small amount of information you've supplied, it sounds like the most probable cause is a wimpy motor. You may need to gear down farther or buy a better motor.

oops sorry here is the source. The PID controller is derived from this tutorial with the exception that I am smoothing the encoder readings with a Kalman filter. 

#include <SimpleKalmanFilter.h>

// Pins
#define ENCA 12 // motor encoder 1
#define ENCB 14 // motor encoder 2
#define PWM 2   // pwm
#define IN1 16  //in 1
#define IN2 15  //in 2

// global
long prevT = 0;

// use volatile inside interrupts 
volatile int pos_i = 0;
volatile long prevT_i = 0;
volatile float velocity_i = 0;

float eintegral = 0;

/*
 SimpleKalmanFilter(e_mea, e_est, q);
 e_mea: Measurement Uncertainty 
 e_est: Estimation Uncertainty 
 q: Process Noise
 */
SimpleKalmanFilter simpleKalmanFilter(2, 2, 0.025);


void ICACHE_RAM_ATTR readEncoder(){
  
  int b = digitalRead(ENCB);
  int increment = 0;
  if(b>0)
  {
    increment = 1;
  }
  else {
    increment = -1;
  }
  pos_i = pos_i+increment;

  long currT = micros();
  float deltaT = ((float)(currT-prevT_i)) / 1.0e6;
  velocity_i = increment/deltaT;
  prevT_i = currT;
  
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(ENCA,INPUT);
  pinMode(ENCB,INPUT);
  pinMode(PWM,OUTPUT);
  pinMode(IN1, OUTPUT);
  // this is only ever used to activate phase-enable mode on the drv8835
  pinMode(IN2,OUTPUT);
  digitalWrite(IN2,HIGH); 

  attachInterrupt(digitalPinToInterrupt(ENCA), readEncoder, RISING);
}




void setMotor(int dir, int pwmVal, int pwm, int in1){

  // **Note: pwr = 0 will stop the motor
  analogWrite(pwm,pwmVal);
  if(dir == 1)
  {
    digitalWrite(in1,HIGH);
  }
  else if(dir == -1)
  { 
    digitalWrite(in1,LOW);
  }
  else
  { // stop-break
  //  Serial.println("motor stopped.."); 
  }
}


void loop() {

  // read motor encoder 
  float velocity2 = 0;
  int pos = 0;
  noInterrupts();
  pos = pos_i;
  velocity2 = velocity_i;
  interrupts();

  // calculate RPM
  float RPM = velocity2/350.0*60;

  // apply kalman filter
  float v1Filt = simpleKalmanFilter.updateEstimate(RPM);

  // set target. Square wave between 0 and 200 (RPM)
  long currT = micros();
  float deltaT = ((float)(currT-prevT))/1.0e6;
  prevT = currT;
  float vt = 180 *(sin(currT*0.5/1e6)>0);
  

  // pid
  float kp = 7;
  float ki = 10;
  float e = vt-v1Filt;
  eintegral = eintegral + e*deltaT;
  float u = kp*e + ki*eintegral;

  int dir = 1;
  if(u<0){
    dir= -1;
  }

  // PWM based on control singlal U
  int pwr = (int) fabs(u);
  if(pwr > 255)
  {
    pwr = 255;
  }
  
  setMotor(dir,pwr,PWM,IN1);
  Serial.print(vt);
  Serial.print(" ");
  Serial.print(v1Filt);
  Serial.println();
  delay(1);
}

Hi @anupamd,

  At a first glance, your program is printing out some internal values, but not (directly) what the motor is being told to do. Hence, I think it is difficult for you to see how your software is reacting.

I would start by adding "pwr" to your print statement, so you can see how much power (on the 0..255 scale) the motor is getting.

If it is near to 255, when it is slowed down with a higher load, then clearly the PID cannot try any harder ... and as has already been suggested, you may need a more powerful motor/electrical power drive.

On the other hand, if it is much lower than 255, then your software isn't reacting in the way you hoped, and needs further attention.

------------

As secondary comments that you can choose to accept or ignore:

  If you are unfamilar with PID's, you might consider starting with just the 'P' ... i.e. Proportional  part, and get that to work as well as you can ... it should be possible to get a reasonable level of feedback control .. many real world systems are based on just proportional control. Then, if you wish progressively add in integral and differential elements to optimise the system.

  Similarly, I think I would have started with a simple run averaging algorithm to determine the motor speed, rather than engaging a Kalman filter, until I was more familiar with the system.

------------

Good luck with your project .. sorry I haven't given you a solution, but hopefully you can simplify and investigate it yourself.

Best wishes, Dave

@DaveE thank you so much for your insight. Starting simple is always the best option when things are not working . I agree that a P controller with a running average will probably be just fine or at the very least it will be good to compare against the current setup. The kalman filter and a full PID does add additional complexity tuning parameters to the system, some of which I dont fully understand (yet). I also agree that printing pwr to show me directly what is being sent to the motors is essential. I am running these experiments now and will report back.

based on specs,  I believe the N20 motors I got from Adafruit are the "LP" (low power) motors

https://www.pololu.com/category/60/micro-metal-gearmotors

From this I definilty think I could go with a high power motor as I have room in my bot for a larger pack and the motors are virtually the same size. So I dont see a downside other than having to upgrade some power requirements. I will put some 1:50 HP motors on order and experiment with those.

All that said, I do need to look at what PWR is doing so on to the bench for some tests. Stay tuned and thanks!

-Anupam