# Using PID algorithm to control 5 motors. How to lighten Void loop

Hello,

I'm not a programmer at all, and I'm just copying and pasting.

I am trying to use a pid algorithm twice to adjust the speed of two motors with encoders.

But I think I have too many things to calculate in Void Loop () which means that my two engines move in a jerky way but when I use one after the other it works perfectly.

In addition, I would like to control 5 in the end.

How to structure my program thank you. (my program is attached)

PREMIERDEUX.ino (8.41 KB)

The code in the PID library is itself very slow because it uses floating point maths. It is also written to be applicable to a wide variety of applications. I suspect if you want to control 5 motors you will need to extract the essence of it into a simpler function.

How many steps per revolution, and how many revolutions per second do you need to deal with? Reading an encoder is hard work for an Arduino. I can control the speed of a small DC motor (up to 15000 RPM, or more) with a single pulse per revolution.

...R

The performance (and stability) of a digital controller is highly dependent on the sample rate.

It could be that your loop is too slow for your particular application, however, it might be that you just didn't tune it correctly for the new sample rate.

You cannot tune the controller with a single motor controller in the loop (sample time = Ts), and then add another motor, doubling the loop time, so the new sample time would be 2Ts.
You have to retune the controller.

Pieter

PieterP:
You have to retune the controller.

If, by that, you mean changing the K values then I am not so sure that it is necessary. My motor control works fine with fixed K values over a speed range from about 1500 to 15000 RPM.

What would be essential is to ensure that the ERROR is in correct proportion to the data being collected. For example if a longer period gives bigger values such that the error number is doubled (say) allowance must be made for the fact that the bigger error number does not mean the mismatch between actual and desired performance is greater.

...R

I'm talking about the sampling time of the controller, not the speed of the motor.

Digital PID controllers usually use the forward Euler numerical integration technique with a fixed sample rate. This means that they can just move this time step factor into the Kd and Ki constants. That's exactly what Arduino PID library does. So changing the sample rate essentially changes the effective tuning parameters.
Even just a factor of two can make the difference between stability and instability.

OP seems to use a sample time of 15 ms. This should be plenty for the PID library to compute the result.
However, it's not at all enough to be sending large strings of text over the Serial port at 9600 baud ...
Either remove the print statements, or pump up the baud rate to 1 Mbaud.

I used to control 4 motors with manual values instead of PID library of Arduino for a drone.

If you are interested in looking at it, you could find it in the attachment of this post.

Good luck!

Transmitter_-_Joystick.ino (861 Bytes)

PieterP:
So changing the sample rate essentially changes the effective tuning parameters.

The sample rate for my DC motor control is the time for one revolution. And, as I said earlier, it works fine from 1500 to 15000 RPM.

...R

Then you got lucky I guess

So changing the sample rate essentially changes the effective tuning parameters.

Exactly! Which is why anyone who looks at the PID source code will find the PID functions for setting the Kp, Ki & Kd coefficients scale the new coefficients based on the current sample rate. Similarly, the function for changing the sample rate wil re-scale the Kp, Ki & Kd coefficients based on the ratio of old and new sample rates. Changing the sample rate, more than very slightly, without re-scaling the coefficients WILL de-tune the PID, and reduce performance. The PID algorithm, particularly calculation of the integral and differential terms, absolutely DEPENDS on having a constant sample rate, and coefficients adjusted FOR that specific sample rate.

Regards,
Ray L.

PieterP:
Then you got lucky I guess

Maybe, but I think it depends. You are 100% right that different sample rates can change how a PID (or any controller) behaves, but changes in sample rate can easily be accounted for.

For instance, for the P element of the controller sample rate doesn't matter at all since nothing in the calculation within that component of the controller relies on time.

The I and D elements of the controller do matter on sample rate, though. Let's focus on I first: If my error at the first timestep (t=0s) is 5. Then lets say there is an error of 10 at the next timestep after an arbitrary amount of time passing (non-fixed sample rate). If we don't account for time, the ouput of the I term is

``````Ki * (10)
``````
• assuming a simple discrete time-integration approach is used. With this approach, you will get wildly different results with different sample rates. BUT, if you generalize the output of the I component as
``````Ki * (error2) * (time2 - time1)
``````

, changing sample rate is automatically accounted for - your controller will act the same nomatter what (within reasonable sample rates of course)

The same is true for the D component -

``````Kd * (error2 - error1) / (time2 - time1)
``````

<-- sample rate is already accounted for in this generic formula

Obviously though, it is best to use the highest sampling rate as possible and trying to keep it a fixed rate is a good practice, too.

You are absolutely correct, but the PID library doesn't check the real elapsed time, it uses a time that the user sets, and then changes the constants.

https://github.com/br3ttb/Arduino-PID-Library/blob/9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb/PID_v1.cpp#L113-L114

As long as the loop time is less than the sample time, all is well, and the library maintains a (more or less) fixed rate. However, if the update rate drops, it doesn't maintain the set rate, and it doesn't correct the constants.

PieterP:
You are absolutely correct, but the PID library doesn't check the real elapsed time, it uses a time that the user sets, and then changes the constants.

https://github.com/br3ttb/Arduino-PID-Library/blob/9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb/PID_v1.cpp#L113-L114

As long as the loop time is less than the sample time, all is well, and the library maintains a (more or less) fixed rate. However, if the update rate drops, it doesn't maintain the set rate, and it doesn't correct the constants.

Arduino-PID-Library/PID_v1.cpp at 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb · br3ttb/Arduino-PID-Library · GitHub

Interesting, I didn't know the PID library was that un-robust, smh

This tutorial enlightens the PID library inner working:
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

PieterP:
Then you got lucky I guess

I don't think there is any luck involved. I successfully tried the same simple system on a servo mechanism and a 3D printer heater.

As far as I can see most of what is written about PID is just pseudo-science - and I spent a considerable time trying to find a proper explanation of the alleged theory. At its heart it is just a simple lick-it-and-hope-it-stops-bleeding system - and if it doesn't then give it another lick (or two or three).

Whoever wrote the PID library could have spent his/her time better by writing a simple explanation of how to control a system - teach them to fish, rather than give them a trout.

...R

The mathematics behind PID and control theory in general are not at all pseudo-science. However, it's not easy to explain in a simple way, especially if you don't know what the mathematical background of your target audience is, or if the target audience doesn't really care about the maths, and they just want to get their project working.

On top of that, some parts of the system's dynamics, like friction, are hard to model correctly, since they have inherent non-linearities.

Using MATLAB with system identification and PID tuning can take a lot of guesswork out of the equation, but that's not something that a complete beginner can easily do.

so everyone,

I changed the frame rate to see the data on the monitor, I set it to 115200 and now both my motors are running normally. I set the same Setpoint with the same Kp, Ki and Kd but they do not run perfectly at the same speed which is quite normal when we look at the two val_oupout they are very different. But at the same time, the speed detected for each motor is the same.

What is strange is that my two encoder engines are strictly identical. They each have two sensors arranged in phase quadratures.

So what could be the problem?

``````latestDurationCounta25.00
Setpointa25.00
val_outputa64.65
--------------------------latestDurationCount25.00
---------------------------Setpoint25.00
---------------------------val_output87.05
latestDurationCounta25.00
Setpointa25.00
val_outputa64.65
--------------------------latestDurationCount26.00
---------------------------Setpoint25.00
---------------------------val_output86.43
latestDurationCounta25.00
Setpointa25.00
val_outputa64.65
``````

PieterP:
The mathematics behind PID and control theory in general are not at all pseudo-science.

Let's agree to differ.

...R

Robin2:
Let's agree to differ.

I'm interested to know what makes you think that control theory is pseudoscience?

bvking:
So what could be the problem?

Both duration measurements seem the same. The controller thinks that it's doing an excellent job. So either the code that measures the duration is wrong, or the measurement is just not accurate enough.

Please clean up your code, it's impossible to read with all the global variables and the duplication of every statement.

Try encapsulating the controllers in a class, and just create two instances of that class.

I have tried to clean up the code and later I 'll try to

encapsulating the controllers in a class, and just create two instances of that class.

I have change the order in the loop between the two "instance"(not sure of this term) of PID but it's always the same motor with a val_ouput higher... I'm working with Arduino Due.

``````#include <PID_v1.h>
//MOTOR D (premier moteur)

const byte encoder0pinA = 50;//A pin -> the interrupt pin 50
const byte encoder0pinB = 51;//B pin -> the digital pin 51

byte encoder0PinALast;
double  latestDurationCount,duration,abs_duration;//Input : the number of the pulses
boolean Direction;//the rotation direction
boolean result;

double val_output;//Power supplied to the motor PWM value.
double Setpoint;
double Kp=0.6, Ki=5, Kd=0; // 0.6 ; 5 ; 0
PID myPID(&latestDurationCount, &val_output, &Setpoint, Kp, Ki, Kd, DIRECT);

//**************************************************
// MOTOR C (deuxieme moteur)

const byte encoder0pinAa = 52;//A pin -> the interrupt pin 53
const byte encoder0pinBa = 53;//B pin -> the digital pin 54

byte encoder0PinALasta;
double  latestDurationCounta,durationa,abs_durationa;//the number of the pulses
boolean Directiona;//the rotation direction
boolean resulta;

double val_outputa;//Power supplied to the motor PWM value.
double Setpointa;
//double Kp=0.6, Ki=5, Kd=0; // 0.6 ; 5 ; 0
PID myPIDa(&latestDurationCounta, &val_outputa, &Setpointa, Kp, Ki, Kd, DIRECT);

//***************************************************

// PORT POUR KIT L298N 2nd (Bleu clair) supllied with 6 V
// PREMIER MOTEUR D

//Ports de commande du moteur D supplied with 6 volt.

int motorPinD1 = 13; // pin to set H BRIDGE of L298N
int motorPinD2 = 12; //12 pin to set H BRIDGE of L298N
const int enablePinD= 11; //11 //  Commande de vitesse moteur, to Output ENA pour L298 the second

// SECOND MOTEUR C

//Ports de commande du moteur C supplied with 6 volt.

int motorPinC1 = 8; // pin to set H BRIDGE of L298N
int motorPinC2 = 9; //12 pin to set H BRIDGE of L298N
const int enablePinC= 10; //11 //  Commande de vitesse moteur, to Output ENA pour L298 the second

void setup()

{
Serial.begin(115200);//Initialize the serial port
/*
NO NEEDED with ARDUINO DUE//// on initialise les entrées pour les encodeurs ?

pinMode(encoder0pinA, INPUT);// encodeur motor D
pinMode(encoder0pinAa INPUT);// encodeur motor C
*/

// Configuration des ports en mode "sortie" C
pinMode(motorPinC1, OUTPUT);   //L298N Control port settings direction of motor C (originaly L298P)
pinMode(motorPinC2, OUTPUT);  //L298N Control port settings direction of motor C
pinMode(enablePinC, OUTPUT);  // powerRate to control speed of motor C

// Configuration des ports en mode "sortie" D
pinMode(motorPinD1, OUTPUT);
pinMode(motorPinD2, OUTPUT);
pinMode(enablePinD, OUTPUT);

// PID motor D

Setpoint=25; // SETPOINT FIRST MOTOR D

myPID.SetMode(AUTOMATIC);//PID is set to automatic mode
myPID.SetSampleTime(10);//Originaly Set PID sampling frequency is 100ms but not precise and motor ramble
EncoderInit();//Initialize the module

// PID motor C

Setpointa=25;

myPIDa.SetMode(AUTOMATIC);//PID is set to automatic mode
myPIDa.SetSampleTime(10);//Originaly Set PID sampling frequency is 100ms but not precise and motor ramble
EncoderInita();//Initialize the module

}

void loop()
{

//****************SETTING MOTOR C

noInterrupts();
latestDurationCounta = durationa;
interrupts();
abs_durationa = abs(latestDurationCounta);

//Setpointa=25;

resulta=myPIDa.Compute();//PID conversion is complete and returns 1
if(resulta)
{

Serial.print("latestDurationCounta"); Serial.println(latestDurationCounta);
durationa = 0; //Count clear, wait for the next count
Serial.print("Setpointa"); Serial.println(Setpointa);
Serial.print("val_outputa"); Serial.println(val_outputa);// volt to MOTOR= real speed
}

//****************SETTING MOTOR D

// Original in the algo to program the COUNT

//   abs_duration=abs(duration);

noInterrupts();
latestDurationCount = duration;
interrupts();
abs_duration = abs(latestDurationCount);

//Setpoint=25;

result=myPID.Compute();//PID conversion is complete and returns 1
if(result)
{
Serial.print("--------------------------latestDurationCount"); Serial.println(latestDurationCount);
duration = 0; //Count clear, wait for the next count
Serial.print("---------------------------Setpoint"); Serial.println(Setpoint);
Serial.print("---------------------------val_output"); Serial.println(val_output);// volt to MOTOR= real speed

}

}

void EncoderInit()
{
Direction = true;//default -> Forward
pinMode(encoder0pinB,INPUT);
attachInterrupt(50, wheelSpeed, CHANGE);
}

void wheelSpeed()
{
if((encoder0PinALast == LOW) && Lstate==HIGH)
{
if(val == LOW && Direction)
{
Direction = false; //Reverse
}
else if(val == HIGH && !Direction)
{
Direction = true;  //Forward
}
}
encoder0PinALast = Lstate;

if(!Direction)  duration++;
else  duration--;

}
{
digitalWrite(motorPinD1,HIGH);
digitalWrite(motorPinD2,LOW);

analogWrite(enablePinD,val_output);
}
void back()//Motor reverse
{
digitalWrite(motorPinD1,LOW);
digitalWrite(motorPinD2,HIGH);

analogWrite(enablePinD,val_output);
}

void Stop()//Motor stops
{
digitalWrite(enablePinD, LOW);
}

void EncoderInita()
{
Directiona = true;//default -> Forward
pinMode(encoder0pinBa,INPUT);
attachInterrupt(52, wheelSpeeda, CHANGE);
}

void wheelSpeeda()
{
if((encoder0PinALasta == LOW) && Lstatea==HIGH)
{
if(vala == LOW && Directiona)
{
Directiona = false; //Reverse
}
else if(vala == HIGH && !Directiona)
{
Directiona = true;  //Forward
}
}
encoder0PinALasta = Lstatea;

if(!Directiona)  durationa++;
else  durationa--;

}
{
digitalWrite(motorPinC1,HIGH);
digitalWrite(motorPinC2,LOW);

analogWrite(enablePinC,val_outputa);
}
void backa()//Motor reverse
{
digitalWrite(motorPinC1,LOW);
digitalWrite(motorPinC2,HIGH);

analogWrite(enablePinC,val_outputa);
}

void Stopa()//Motor stops
{
digitalWrite(enablePinC, LOW);
}
``````