Dear friends, can I ask how is the pid constants calculated? Ty

They are not calculated. They are experimentally determined.

The PID

**proportional**

proportional is simple it is just a differential using error. if you make a change in the input you get an immediate change in the output

PTerm is added to the output.

if proportional is 1 and the input changes by 10 our output changes by 10.

proportional is a floating point number so you can use fractions for detailed tuning.

```
// Proportional term
double PTerm = kp * error;
```

**integral**

integral uses time and adds a portion of the error to the ITerm and should be used to help land setpoint.

if the output produces too little heat to achieve room temperature setpoint after the proportional term settles your integral term will be added to every cycle to provide more and more heat to reach setpoint. The integral is based on 1 second of time but you can devide that time change up into fractions of a second to make the changes more detailed at that time. ITerm is added to the output.

```
// Integral term
integral += error * (double) (timeChange * .000001);
ITerm = ki * integral;
```

**derivative**

derivative is most complex but it isn’t hard to understand. most of us don’t even need derivative until we are fine tuning and using derivative during initial tuning may cause difficulties. derivative uses the rate of change to temporary add or subtract from the output to alter course. if you were driving a car and heading for a cliff derivative term would kick in and make a drastic temporary adjustment. example derivative happens when a door is opened to a cold winter night and cold air is let in the sensor sees the air and derivative sees the quick change in temperature and turns on the heat a bit more than the proportional would do by its self. but once the change has settled down ( door left open ) derivative disappears from the equation. when the door is closed the same thing happens in reverse. the temperature starts rising and derivative shuts off the heat early trying to keep you from overshooting setpoint. derivative is also based on 1 second and can be divided

```
// Derivative term using angle change
derivative = (input - lastInput) / (double)(timeChange * .000001);
DTerm = (-kd * derivative);
```

**The Output:**

lets just add them together

```
//Compute PID Output
double output = PTerm + ITerm + DTerm ;
```

everything else is to prevent the output from exceeding limits or preventing what is called windup with the integral term.

Note: timeChange is in microseconds and is multiplied by .000001 to change it to seconds.

My PID Calcuate code:

```
unsigned long now = micros();
unsigned long timeChange = (now - lastTime);
if (timeChange >= (SampleTime * 1000)) // Minimum Sample Time
{
double DTerm = 0;
double derivative = 0;
// kp - proportional gain
// ki - Integral gain
// kd - derivative gain
// dt - loop interval time
// outMax - maximum value of manipulated variable
// outMin - minimum value of manipulated variable
// Calculate error
double error = Setpoint - input;
// Proportional term
double PTerm = kp * error;
// Integral term
integral += error * (double) (timeChange * .000001); // uses real delta T not a fixed delta T
ITerm = ki * integral;
if ((ITerm > outMax) || (ITerm < outMin) ) integral -= error * (double) (timeChange * .000001); // Prevemts Windup
/*
///////////////////////////////////////////////////////////////////////////////////////////////////
We were not talking about that but it's something clever:
....Jerky robot, that's because the Kd * de(t)/dt term goes to infinity when you change the set-point. Try instead of using the angle error, e(t), use the negative of the actual value change, dangle(t)/dt. they are equivalent but this way you avoid infinity errors.
Kd*de(t)/dt = -Kd*dangle(t)/dt
///////////////////////////////////////////////////////////////////////////////////////////////////
*/
// Derivative term using error change
// derivative = (error - pre_error) / (double)(timeChange * .000001); // uses real delta T not a fixed delta T
// DTerm = kd * derivative;
// Derivative term using angle change
if (kd) {
derivative = (input - lastInput) / (double)(timeChange * .000001); // uses real delta T not a fixed delta T
DTerm = (-kd * derivative);
}
//Compute PID Output
double output = PTerm + ITerm + DTerm ;
//output = max(outMin,output);
if (output > outMax) output = outMax;
else if (output < outMin) output = outMin;
//Remember some variables for next time
pre_error = error;
lastInput = input;
lastTime = now;
// Debugging
#ifdef DEBUG
static long QTimer = millis();
if ((long)( millis() - QTimer ) >= 100) { // Spam Delay at 100 miliseconds
QTimer = millis() ;
char S[10];
//Serial.print(F("\tSkipCtr ")); Serial.print(SkipCtr);
Serial.print(F("\tIn ")); Serial.print(dtostrf(input, 6, 2, S));
Serial.print(F("\tSetpt ")); Serial.print(dtostrf(Setpoint, 6, 2, S));
Serial.print(F("\t/\\T ")); Serial.print(dtostrf(timeChange, 6, 2, S));
Serial.print(F("\tKp ")); Serial.print(dtostrf(PTerm, 6, 2, S));
Serial.print(F("\tKi ")); Serial.print(dtostrf(ITerm, 6, 2, S));
//Serial.print(F("\tKd ")); Serial.print(dtostrf(kd,6,2,S));
//Serial.print(F("\tderivative ")); Serial.print(dtostrf(derivative,6,2,S));
Serial.print(F("\tKd ")); Serial.print(dtostrf(DTerm, 9, 0, S));
Serial.print(F("\tOut ")); Serial.print((int)output);
Serial.println();
}
#endif
}
```

This is an example of tuning PID for a balancing bot:

YouTube: https://youtu.be/uyHdyF0_BFo

What is not mentioned in that video is how the input and output values relate to the K values. It would be more useful if the program code was available for study.

You may (or may not) be interested in this Thread which explored the subject.

...R

Robin2:

What is not mentioned in that video is how the input and output values relate to the K values. It would be more useful if the program code was available for study.

The “My PID Calcuate code” example is the core of the calculation but since you asked the code I am using is attached below. this is the code seen running with the balancing bot. With the balancing bot example, the attached PID_V2 code is triggered about every 10 milliseconds when the MPU6050 completes the DMP calculations (This happens every 9.98 milliseconds to 10.01 milliseconds).

Now for my disclaimer The code is a modification of the well known PID_V1 everyone seems to be using. The V1 code works fine ONLY if you have a FIXED time window between readings. otherwise it fails to manage exact control of the PID loop.

What PID_V2 does differently: Simply I can have a variable duration between calculations. I am also calculating down to the microsecond which dramatically improved the response of my balancing bot PID and simplified tuning. I have also used this to manage controlling a tachometer with 400 steps per revolution at speeds up to 2000 rpm! The tachometer changes duration between readings constantly which PID_V1 couldn’t handle.

The PID_V2 has a serial debug readout which is wonderful in troubleshooting your PID just add #define DEBUG to the top of your code:

```
#define DEBUG
```

I don’t want to have this thread become distracted from what was asked so lets avoid bashing my code here.

Thanks

PID_v2.cpp (9.19 KB)

MPU6050 - Copy.h (3.08 KB)

zhomeslice: The "My PID Calcuate code" example is the core of the calculation [/quote] Sorry. My mistake. I had not realized it was your video.

...R

Robin2:

Sorry. My mistake. I had not realized it was your video.…R

No Problems Hope this can help.

I’m reviewing your link also

You may (or may not) be interested in this Thread which explored the subject.

It looks to have excellent descriptions about PID

gabrielislost

Please feel free to ask more of us as this is something I enjoy discussing.

Have we answered your question?

zhomeslice: The "My PID Calcuate code" example is the core of the calculation but since you asked the code I am using is attached below.

I would like to study this further because it is a worked example for the control of an electric motor, and is also a system that requires a quck response - unlike (say) a heating system.

However when I looked more closely I realized that the code you have posted is only for your PID capability.

Would you be prepared to share the entire code for your balancing bot so that I can understand the context in which the PID is applied.

...R

Robin2:

I would like to study this further because it is a worked example for the control of an electric motor, and is also a system that requires a quck response - unlike (say) a heating system.However when I looked more closely I realized that the code you have posted is only for your PID capability.

Would you be prepared to share the entire code for your balancing bot so that I can understand the context in which the PID is applied.

…R

Yea Sure

The following bot code is what I used for the 90° turning control at the end of the video

full Video of 90° drive/turn https://youtu.be/jv3LOUeeyAI

be sure to use the PID_v2 that I created or you will never achieve balance!

Turning code:

```
//code for Automated Turning and forward Motion
static long QTimer = millis();
if ((long)( millis() - QTimer ) >= 5000) {
QTimer = millis();
StopTime = millis();
SPAdjust = 3;
YawOffset += 90;
YawOffset = (((int)((YawOffset * 10) + 1800)) % 1800) * .1;
if (YawOffset > 180) YawOffset -= 360; // rollover to negative
if (YawOffset < -180) YawOffset += 360; // rollover to positive
}
if ((long)( millis() - StopTime ) >= 200 )SPAdjust = 0;
```

BalancingBotUnoOrMegaControlable2.ino (31.5 KB)

Thanks for that. Much appreciated. I will take a bit of time to try to understand it. At the moment I don't have any plans to build a copy. (I have no interest in football myself :) )

Am I correct to assume that #include "MPU6050_6Axis_MotionApps20.h" refers to a standard library rather than something you have written?

...R

Robin2:

Thanks for that. Much appreciated. I will take a bit of time to try to understand it. At the moment I don’t have any plans to build a copy. (I have no interest in football myself )Am I correct to assume that #include “MPU6050_6Axis_MotionApps20.h” refers to a standard library rather than something you have written?

…R

Correct all other libraries are standard and work well

I have now had time to study your code and I hope you don't mind a few follow-up questions.

- Am I correct to think that the primary driver of the system is the interrupts produced by the MPU?
- If so, do you have any idea how often they occur?
- It seems that the principal inputs to PID.compute() are YawInput, Turn and DegSetpoint. Can you indicate what ranges each of these values can take?

Thanks.

...R

Robin2: I have now had time to study your code and I hope you don't mind a few follow-up questions.

Of Course and thank you for your imput into my delima with structures. :)

Am I correct to think that the primary driver of the system is the interrupts produced by the MPU?

yes The MPU stuffs a FIFO buffer with all ready calculated angles in radians. if you wait too long to get the input, you get old data or corrupted data then you have to reset and wait for the next calculation. so the interrupt tells the UNO program to get the data over i2c and the best part is it is already for you to use.

If so, do you have any idea how often they occur?

This by default of the program is at about 10 milliseconds and it is set in the file MPU6050_6Axis_MotionApps20.h at about line 305

```
0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
// It is important to make sure the host processor can keep up with reading and processing
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
```

It seems that the principal inputs to PID.compute() are YawInput, Turn and DegSetpoint. Can you indicate what ranges each of these values can take?

The output from the FIFO buffer is in radians easily converted to degrees I may have shifted the degree by a decimal or two rather than using a float. So 180 degrees would be 1800 this gives me an extra level of control without sacrificing processing time. I convert it back for display.

The process is: The MPU triggers the interrupt pin. The interrupt sets flag. Mack to the main program, where ASAP we get the last (newest) FIFO data, calculate radions to degrees, process PID, then set outputs. The faster this happens the less lag you face in controlling balance. As we've discussed changing the PID code from using a fixed time interval to a variable time interval allows for variations in the delay between interrupts.

Thanks.

...R

Welcome Z

Thanks again.

I now need to think about the "problem" of how a robot falls over (or does not) and see can I draw parallels with my small DC motor control.

One obvious difference is that in your system you are measuring angles with the MPU and controlling a motor whereas I want to measure and control the motor. Also your system will have a great deal more inertia than mine.

In many ways it seems that "PID", like "State Machine", is a fancy name that makes some fairly simple concepts seem very esoteric.

...R

Robin2:

Thanks again.I now need to think about the “problem” of how a robot falls over (or does not) and see can I draw parallels with my small DC motor control.

Start Balancing is in the loop() function

```
void loop() {
//.... other code for startup
// Input = (ypr[1] * 180 / M_PI) * 10; // the * 10 shifts the decimal place by 1, so 1° is actually int 10
// and 180° is actually 1800 and this calculates so that 0 is level going +-x°'s is out of level
// InputABS = abs(Input); is found in the MPUMath function and it focuses on being balanced remember that I
// shifted the decimal by 1 so 10 is actually 1.0°
if (InputABS < 10) { // if we are within 1° of level lets start the PID for balancing
Mode = true;
myPID.SetMode(AUTOMATIC);
myTurnPID.SetMode(AUTOMATIC);
}
// other code
}
```

Code to kill the motors because we are giving up on balancing is found in the MPUMath funciton

```
void MPUMath() {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Input = (ypr[1] * 180 / M_PI) * 10; // Shift by * 10 after calculating degrees +-180
InputABS = abs(Input); // Get the offset from level as an absolute number
if (InputABS > 450) { // at 45.0 degrees Kill balance and give up trying <<<<
Mode = false;
myPID.SetMode(MANUAL);
myTurnPID.SetMode(MANUAL);
Output = 0;
Turn = 0;
FullBreaking ();
return;
}
// Other Code
}
```

I wait for the MPU to say we are close to balanced before starting the PID control

One obvious difference is that in your system you are measuring angles with the MPU and controlling a motor whereas I want to measure and control the motor. Also your system will have a great deal more inertia than mine.

I’m unsure of what you are attempting to accomplish but if it is similar, lets see what we can do.

In many ways it seems that “PID”, like “State Machine”, is a fancy name that makes some fairly simple concepts seem very esoteric.

…R

You welcome

Z

You are very kind, and I must apologize. I had not intended “I now need to think about the “problem” of how a robot falls over (or does not) and see can I draw parallels with my small DC motor control.” as a question.

What I have in mind is for me to try to understand the physical process rather than the code.

…R

You can calculate the filter constants for a controller if you can model the system. For a balancing robot, the hard part is characterizing the motor and solving the Lagrangian equations describing the robot. I did an analysis of the problem that you can find here…

http://nxtotherway.webs.com

I used this model for a lego nxt robot and an arduino controlled robot.

charliesixpack: and solving the Lagrangian equations

I would be far out of my mathematical depth.

...R

charliesixpack, Do you have the Lagrangian equations as a C code you could share? I struggle reading lego nxt Z

Dear zhomeslice and Robin2 thanks for your reply, I am really a beginner at this as i am trying to do this self balancing robot project and i am trying to use PID to control the stepper motors of the robot, i am still trying to understand the code as i only know abit about arduino, please help haha. And is it normal for my values of the mpu to go back and forth when changing angles? and is will defining debug solve that problem? Hope to hear from you guys again thanks:) Sincerely Gabriel