Using PID Controller for Stable Platform

So I’m trying to learn PID controllers for another project and to do so I decided to make a simple ‘auto-stabilising’ platform.

The idea being a gyro/accel (MPU-6050) measures the angle the system is at, runs the PID controller based on a target setpoint (i.e 90 degrees) and then writes a new position to the servo. Thats the plan anyway.

My code results in huge errors and the output (servo position) maxes out almost immediately. I’m not sure if im using the PID() function wrong, or if theres some other stupid mistake in there.

NB. I know that a PID might be overkill for this sort of thing but as I mentioned above, this is a small part of a much larger project. Eventually I am going to need two servo’s which respond to serial inputs to move themselves to a desired angle, and then use the MPU or similar in a control loop to ensure they are at the correct angles. I have build most of this, except for the control system, hence why I am doing this

//Include Libs
#include <Wire.h>
#include <Servo.h>
#include <MPU6050_tockn.h>
#include <PID_v1.h>

//Define Constants
const int servo_pin = 3;
const double Kp = 2, Ki = 5, Kd = 1;                                //PID controller values
double setpoint = 90;                                               //Target Position

//Define Variables
double servo_pos, y_angle;                                          //Servo Position and Measured Y angle

//Create Servo, MPU and PID objects
Servo servo_1;
MPU6050 MPU_1(Wire, 0.1, 0.9);                                      //0.1 & 0.9 are percentages of Accel and Gyro angle used
PID PID_1(&y_angle, &servo_pos, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);                                             //For Debugging and Displaying results
  servo_1.attach(servo_pin);                                        //Attach Servo
  servo_pos = setpoint;
  servo_1.write(servo_pos);                                         //Turn Servo to Desired Angle to start
  Wire.begin();                                                     //Begin I2C comms.
  MPU_1.begin();                                                    //Start Comms with MPU-6050
  MPU_1.calcGyroOffsets(true);                                      //Run Gyro Calibration
  PID_1.SetMode(AUTOMATIC);                                         //Start PID

void loop() {
    MPU_1.update();                                                 //Get Data From MPU
    y_angle = MPU_1.getAngleY() + 90;                               //Calculate angle as raw angle + 90
    PID_1.Compute();                                                //Compute PID               
   servo_1.write(servo_pos);                                       //Write PID Output to Servo

    Serial.print("Input Angle: ");Serial.print(setpoint);
    Serial.print("\tMeasured Angle: ");Serial.print(y_angle);
    Serial.print("\tServo Position: ");Serial.println(servo_pos);


const double Kp = 2, Ki = 5, Kd = 1;

Pretty big values. How did you arrive at them?

literally googled typical pid numbers. I know theres a autotune PID library somewhere, might give that a go.

To be honest I assumed the issue was with my code not the numbers.

To be honest I assumed the issue was with my code not the numbers.

That your system quickly goes out of control suggests otherwise.

To be honest I assumed the issue was with my code not the numbers.

Assume both are issues. Sign errors are common.

Put in lots of print statements for testing and see if the input and output values make sense, for a given platform tilt.

a simple 'auto-stabilising' platform

No such thing.

I have an X/Y stabilized platform based on the MPU6050, servos, and the Arduino Due; yea the Due is doing other things.

The MPU6050 sits on the platform base and is read by a RPi, the RPi converts the values into wtiteMicroseconds values, sends them to the Due and the Due does the servo torque gig.

The servos are mounted so that they are leveled at near 90 degrees and have a range of 1000 to 2000 microseconds. The servos are being updated 6X a second and, using millis, each servo is given 20 milliseconds to make apply a torque. The servos are limited to a max torque value of 3 degrees per torque event. With a limit of 1000uS and a max torque range of 1000uS, each degree is 11.11uSec. I am torquing a minimum of 11.11/5.

The RPi is doing the complementary filtering. I mention the complementary filter because I highly recommend some type of filtering of the MPU6050 data. I gathered MPU6050 data into a file (csv) and used a spreadsheet to view the data. The noise level of your MPU can been seen, before filtering and after filtering to give you an idea of how close to level the microsecond torque values and still be meaningful, without dropping into the noise and trying to torque out the noise.

I use, for the complementary filter both accelerometer and gyro angle info. The gyro will give you the value of the last precession the gyro felt, the accelerometer will give you the current precession that is being felt. Thus, from moment to moment, the gyro is slightly behind the current platform angle. Thus, an integration with the current accelerometer value will help keep the platform in near real time level.

Torquing in the MPU6050 noise levels will, eventually, cause platform oscillations.

Also, it is important to set up the MPU6050. Such as setting the clock to lock to a PLL, I use the Xgyro, instead of using the free running internal clock. The sample rate should be set,, useful in the complementary filter calculations.

The gyro and accel config values should be considered Set the sensitivity to high and the platform will tend towards oscillations, too low and the MPU could miss events. These setting are used for your scale factors so knowing these values is important.

Once every 24 hours, automatically, a calibration routine is run to get Gyro offsets, Acelerometer offsets, x and y rotation off sets, and gyro drift rates. Without proper offsets applied the MPU info will drift. I found that any calibration samples below 50 to be useless and above 500 to be unnecessary. I use 300 samples for the gyro / accelerometer offsets, x/y rotation offsets, and the gyro drift rate offsets. Note, during the offset calibration routines, do not apply the current offsets or drift rates to your data samples. The Gyro /acceleremeter and x/y drift rate calculation can be done at the same time, the gyro drift rate offset must be done after the offset calculations. The drift rate calculation uses the offset calculations. My offset and drift rate routines change the signs of the calculations.

Accelerometer drift rates are applied (added to) during scale factor measurement. Gyro offsets and drift rates are applied (added) after scale factor calculations.

Oh, It is important not to skip the Z gryo and Z accelerommeter offsets and drift rates. Most especially the Z accelerometer as the Z accelerometer is used in the calculation of accelerometer angle.

Without offsets and drift rates the data from the MPU, quickly, becomes useless.

The point I am hoping to make is that, based upon your shown code, their should be a bit more work done on the data from the MPU before sending it to the servos for torquing. Remember the gyro is giving you the last moments angle reading not the now.

I am not familiar with a PID controller, I'm not using one.

I am not familiar with a PID controller, I'm not using one.

It sounds very much like you are using the "P" of a PID controller.

In other words, output to servo is proportional to the platform angle (error from zero).

So there you are. You've finally 2s complemented, scaled, offset, made error and drift rate corrections to get a number that looks like -0.007175552519084991, Which looks nothing like a '1' or a 1540, numbers that represent a servo torque value.

Right off the bat, you may consider that if the platform is off by 1/5 of a degree, a 1 is just not going to cut it and may very well start the platform oscillating. Thus it is reasonable that you'll want to convert -0.007175552519084991 into a uSec torque.

Now, just because I have the number sitting in front of me on a screen, you have limited the servo travel to +/- 45 degrees on either side of 90 degrees. With a range of 2000uSec to reresent 90 degrees that s 11.11 uSec per degree.

If you are not going to use a complementary filter, you will use the Accelerometer values to get current platform angle. Take this number, -0.007175552519084991; which is either a complementary filter angle value or a accelerometer derived tilt angle value, and multiply it by 11.11 to get your magnitude and direction of torque such as:

fltTorqueAngle = LastY * fltTorqueAngle_uSec

Where LastY represents either the current complementary filter value or the current accelerometer derived platform angle value.

Next, you did save the initial platform set value?

fltIdeal += fltTorqueAngle

and, after rounding the number (fltIdeal) and setting the number (fltIdeal) to an int, send that value to the servo as a uSec Torque value.

Save the new platform angle for the next incoming data set.

I save all incoming torque angle but only torque the servo when the new torque angle exceeds a preset value. I, using millis, also prevent another torque value from being sent to the servo for 20 millis to give the servo time to get there and to prevent oscillations from occurring. So, whiles new torque values are being thrown at the write to servo, they are being ignored for 20 millis.

A plastic geared servo, pummeled with many servo corrections per second, will last about a week, use a metal geared servo.