How is speed obtained from encoder pulse count for use in PID?

Projects General Guidance
1

Hello,

I have been struggling for many days to understand how and why this code works. I am using an Arduino UNO R3 and 2 DC geared motors with encoders on my robot to calculate its distance travelled as well as its speed. Then, i'm using a PID control loop, with P and I terms only, to find the PWM output to the motors. I am using the encoders to find the distance in terms of pulses and then the speed, in terms of pulses per 40 ms.

These are the important parts of the code (the rest is just global variable declarations).

void setup() 
{
  pinMode(motor_AIN1,OUTPUT);       
  pinMode(motor_AIN2,OUTPUT);
  pinMode(motor_BIN1,OUTPUT);
  pinMode(motor_BIN2,OUTPUT);
  pinMode(motor_PWMA,OUTPUT);
  pinMode(motor_PWMB,OUTPUT);

  pinMode(EncoderPinA, INPUT_PULLUP);  //speed encoder input
  pinMode(EncoderPinB, INPUT_PULLUP);
  
  Serial.begin(9600);                      //open the serial monitor, set the baud rate to 9600

  attachInterrupt(digitalPinToInterrupt(EncoderPinA), encoderFunctionA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(EncoderPinB), encoderFunctionB, CHANGE);
  
  FlexiTimer2::set(40, timerISR);    //run timerISR function every time interval in ms
  FlexiTimer2::start();             //start timer interrupt

//left speed encoder count
void encoderFunctionA() 
{
  encoderTicks_A++;
} 
//right speed encoder count
void encoderFunctionB() 
{
  encoderTicks_B++;
}

void countEncoderTicks()
{
  interval_countA = encoderTicks_A;
  interval_countB = encoderTicks_B;
  
  encoderTicks_A = 0;     //clear encoder count values
  encoderTicks_B = 0;
  
  if ((pwm1 < 0) && (pwm2 < 0))
  {
    interval_countA = -interval_countA;
    interval_countB = -interval_countB;
  }
  else if ((pwm1 > 0) && (pwm2 > 0))
  {
    interval_countA = interval_countA;
    interval_countB = interval_countB;
  }
  else if ((pwm1 < 0) && (pwm2 > 0))
  {
    
    interval_countA = -interval_countA;
    interval_countB = interval_countB;
  }
  else if ((pwm1 > 0) && (pwm2 < 0))
  {
    
    interval_countA = interval_countA;
    interval_countB = -interval_countB;
  }
  
  totalTicksA += interval_countA;
  totalTicksB += interval_countB;
}

void speed_PI()
{
  float average_pulses = (totalTicksA + totalTicksB) * 1.0;  //this line works but does not make sense!!!
  //float average_pulses = (totalTicksA + totalTicksB) / 2.0; //this should be the right one, but it does not work!!
  totalTicksA = totalTicksB = 0;      //clear

  positions += average_pulses;
  positions += forward;             //Forward control fusion, with the values sent by user via serial.
  positions += backward;              //Backward control fusion
  
  PI_pwm = ki_speed * (setpoint0 - positions) + kp_speed * (setpoint0 - average_pulses);
}

But, how can the average_pulses be the speed but at the same time also the distance?? It makes sense for the total pulses to be the distance or 'positions' variable in the code, but i can't figure out how the same value can also be used for the speed. This function is executed by the timer interrupt every 40 ms. Is there some kind of magic maths happening inside this timed function for the PI calculation??

I did some research and i found the PID library for Arduino and the author of that library has made a website to explain how it works and i'm wondering if maybe this part relates to my problem? link

2

The way the ki portion of the controller is written rather oddly (not accounting for the forward/backward fusion)

Normal ki portions of PID controllers look kind of like this:

ki_component = ki * (integrated_error + ((previous_error + current_error) / 2) * sample_period)

where integrated_error is the sum of all previous sample errors: SUM(((error_n-1 + error_n) / 2) * sample_period) where n=all samples previous to the current one and such that sample_period is constant

Idk if that helps, but it's good to keep in mind.

Also, assume the total number of pulses represents distance. With this in mind, you'll notice that the integration of this "distance" becomes speed. That may be the source of your confusion - the variable is not used (directly) as both "speed" and "distance" in the first place!

3

Power_Broker:
Also, assume the total number of pulses represents distance. With this in mind, you'll notice that the integration of this "distance" becomes speed. That may be the source of your confusion - the variable is not used (directly) as both "speed" and "distance" in the first place!

That was really helpful. I think i'm starting to see a glimmer of light in this long and dark episode... I still have some questions about how the speed is being obtained from the distance which is represented by the number of pulses, since there are no calculations done with time, dt. Here, dt or the time interval is 40 ms, or 0.04 seconds. So, the proper way, based on my understanding, to find speed is by using the following formula: (average_pulses / dt). However, no such calculation were made and instead the speed is set as directly the number of pulses recorded in 40 ms.

This line: positions += average_pulses, makes sense now as it is summing up the number of pulses recorded every 40 ms, so it's doing integration which is like adding the area under a graph of speed against time, to find the distance.

But, what if... there is an assumption being made, by saying that the average pulses recorded every 40 ms is considered to be the equivalent of the speed. Then, this assumption can be correct in the sense that since dt is always constant, then the speed value is always a multiple (or, a division by 0.04) of 1/0.04. Is this the correct way of thinking about this? On the other hand, if this is true, then the distance should be calculated as a cumulative sum of (average_pulses * dt)... I think i'm lost.

Edit: Oh, i just had another thought... maybe a revelation? I think here, the assumption is that the speed is found by (average_pulses/dt) but dt is considered to be 1 second instead of the loop time which is set by the timer interrupt to be 40 ms, so that would then give a value of speed = average_pulses. Then, this would also solve this line: positions += average_pulses, which would be the sum of the average_pulses per the assumed 1s time interval, so the area under the speed graph would then be, average_pulses*1second = average_pulses.
My understanding based on this idea, is that the 1 second interval was taken instead of the actual time interval of 40 ms, in order to save on calculations or CPU cycles by the limited performance of the (slow?) 16 MHz Arduino UNO R3. I don't know if i'm getting ahead of myself here...

And i have another question about how is speed obtained from this: float average_pulses = (totalTicksA + totalTicksB) * 1.0;
Why just sum up the encoder ticks from both motors? Isn't the speed obtained by adding both pulses and then dividing by 2?

4

DryRun:
So, the proper way, based on my understanding, to find speed is by using the following formula: (average_pulses / dt). However, no such calculation were made and instead the speed is set as directly the number of pulses recorded in 40 ms.

I haven't looked at your code but if the time period is constant then the number of pulses within that period will vary in exactly the same proportion as pulses/time because time is a constant.

Think of a simpler example. You can tell who is biggest if you have a list of people's heights. You can do exactly the same thing if the list is actually their heights / 2 (or heights * 2)

...R

5

I think I'm equally lost tbh. Where is that PID function coming from? Did you write it or did you get it from somewhere?

6

Robin2:
I haven't looked at your code but if the time period is constant then the number of pulses within that period will vary in exactly the same proportion as pulses/time because time is a constant.

Think of a simpler example. You can tell who is biggest if you have a list of people's heights. You can do exactly the same thing if the list is actually their heights / 2 (or heights * 2)

...R

That was quite enlightening. I tried to do some basic calculations:

//this is the speed with timing calculation
 average_speed = (average_pulses / 0.04);  //timing interval of 40 ms.
 
//calculating of distance using average_speed and same timing interval 
 positions = positions + (average_speed*0.04);
//this is equivalent to: 
positions = positions + average_pulses;

This means that the 'positions' value is not affected by the timing interval. It's only the average_speed which will change. Since the time interval is fixed value for every loop iteration, any change in the time interval value will require a corresponding change in the value of kp_speed. The end result is the same, therefore taking into account the time interval in the calculation makes no difference.

I think i got it, finally?

Power_Broker:
I think I'm equally lost tbh. Where is that PID function coming from? Did you write it or did you get it from somewhere?

It's part of my assignment and the code was provided and tested in the lab on the robot and it works. I tried changing a few things but it just stops working or behaves differently with wrong results. I'm trying to wrap my mind around the idea or thought process behind the code... it's kinda like some programming detective work. :slight_smile:

7

There is a nice simple PID function in this link

...R

8

Robin2:
There is a nice simple PID function in this link

...R

Interesting! I will have a look. I might be able to use it in another project. But for my current lab assignment, i don't think that i'm expected to rewrite or change the code, as everything has been tuned, like the Kp and Ki parameters. But i think i understood the reason why the time interval is not mentioned in any of the calculations.

However, i am still wondering about this...

DryRun:
And i have another question about how is speed obtained from this:

float average_pulses = (totalTicksA + totalTicksB) * 1.0;

Why just sum up the encoder ticks from both motors? Isn't the speed obtained by adding both pulses and then dividing by 2?

9

The difference between multiplying by 1 and 1/2 is a simple DC gain, which means you need to "re-tune" your controller. So, yes, they calculated the average wrong, but you can't just introduce a 1/2 gain without having to re-tune kp and ki. I hope this clears it up

10

Power_Broker:
but you can't just introduce a 1/2 gain without having to re-tune kp and ki.

I think you mean, only Kp? As, Ki seems to be unaffected by a change of the time interval in the calculation. I tried to proved it with some basic calculations, as shown in the last line:

DryRun:

//this is the speed with timing calculation

average_speed = (average_pulses / 0.04);  //timing interval of 40 ms.

//calculating of distance using average_speed and same timing interval
positions = positions + (average_speed*0.04);
//this is equivalent to:
positions = positions + average_pulses;

11

No, both ki and kp: the 1/2 gain would change the values of "positions" and "average_pulses"

12

DryRun:
But, how can the average_pulses be the speed but at the same time also the distance??

If the pulses are measured over a know amount of time, for example one second, the count is both the distance (count * movement per pulse) and speed (distance per second).

13

johnwasser:
If the pulses are measured over a know amount of time, for example one second, the count is both the distance (count * movement per pulse) and speed (distance per second).

I don't think it's true if the time interval between pulse measurements is not 1 second. The function is set to run at a time interval of 40 ms. So, if the 'average_pulses' variable represents the distance travelled by the robot in 40 ms, then the 'average_speed' of the robot would be: (average_pulses / 40 ms) in that time interval. So, the total_distance (initialised to zero) travelled with respect to time is given by: total_distance += (average_speed * 40 ms). This is equivalent to: total_distance += average_pulses. In conclusion, the average_speed is dependent on time interval but not the total_distance.

I have made an attempt to rewrite the related function to include those changes.

//global variables
float total_distance = 0;

//speed controller function, executed every 40 ms by timer interrupt
void speed_PI()
{
 //totalTicksA and totalTicksB are the respective total encoder counts for each encoder/motor.
 float average_pulses = (totalTicksA + totalTicksB) / 2.0; //this is the average distance every 40 ms.
 
 totalTicksA = totalTicksB = 0;  //clear both encoder counter variables.

 average_speed = (average_pulses / 0.04); //40 ms loop time.
 
 total_distance += average_pulses;  //equivalent to: total_distance += (average_speed * 40 ms)
 total_distance += forward;             //Forward control fusion, with the values sent by user via serial.
 total_distance += backward;          //Backward control fusion

 PI_pwm = ki_speed * (setpoint0 - total_distance) + kp_speed * (setpoint0 - average_speed);
}

I hope this is correct? If it is, then the time interval does matter for the average_speed which is not equal to the average_pulses, which is the equivalent distance. From my understanding, i don't think that using 1s interval instead of 40 ms to simplify the calculations is justified.