MPU6050 Use a PID to improve the stabilization of a platform

Projects Programming
1

Good evening,
I have to do some kind of platform to stabilize a camera using an arduino UNO, a servo motor and an MPU6050.
I manage to make sure that the "camera" stays upright but I have to improve it thanks to the use of a PID.
I did some research on the internet to get this code, but I really do not think it is correct. I have to test the stabilization for Ki = Kd = 0, then Kp = Kd = 0 and finally Kp = Ki = 0.
I do not understand what to put as value in "setpoint"
Could you enlighten me?
thank you in advance
PS : Sorry for my english i'm french ^^

#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
#include <PID_v1.h>
   
#define _DEBUG false 
//-------------Declarations capteur et servo----------------
Servo servo1, servo2;         
int servo1_pin = 11;
int servo2_pin = 10;
MPU6050 sensor ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;
//---------PID--------------
double input;
double output;
double setpoint;
double Kp=0, Ki=10, Kd=0;

PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, REVERSE);



void setup ( )
{
servo1.attach ( servo1_pin );
servo2.attach ( servo2_pin );
Wire.begin ( );
Serial.begin  (9600);
Serial.println  ( "Initializing the sensor" );
sensor.initialize ( );
Serial.println (sensor.testConnection ( ) ? "Successfully Connected" : "Connection failed");
delay (1000);
Serial.println ( "Taking Values from the sensor" );
setpoint=90;
myPID.SetMode(AUTOMATIC);
myPID.SetTunings(Kp,Ki,Kd);
myPID.SetOutputLimits(0, 180);

delay (1000);
}




void loop ( )
{
sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
input=map (ay, 17000, -17000, 0, 180) ;
myPID.Compute();
servo1.write(output);
Serial.println(output);

//delay(200);

}
2

Have you looked here GitHub - br3ttb/Arduino-PID-Library, where you could see this guy Arduino-PID-Library/PID_v1.cpp at master · br3ttb/Arduino-PID-Library · GitHub , and then this guy will show its self "PID::PID(double* Input, double* Output, double* Setpoint,
double Kp, double Ki, double Kd, int POn, int ControllerDirection), which will lead so forth and so on and understanding of this or that.

3

Yes I was going to see these pages precisely but I still have no idea how to apply a PID for the stabilization :confused:

4

Setpoint is where you want the camera to point. Roll setpoint is almost always going to be zero. Pitch will start out at zero but may want to pitch up or down later.

5

Ok thanks. Do you know why when I test my code in practice it does not work? I really do not see what is wrong even with setpoint = 0

6

You mapped your input from a balanced +/- range to 0-180. That is unnecessary.

"Does not work" is not a sufficient description of the problem. What does it actually do?

7

The servo motor is too sensitive, the angle is higher than the one I give to my platform

8

PID changes the output to make the input closer to the setpoint. Making your setpoint 90 in a range of 0 to 180 is the same as making it 0 in a range of -17000 to +17000.

Since you never change setpoint, the platform should not move after it has found the setpoint, so there is no angle to be higher or lower.

9

But I want the angle to change because it's supposed to stabilize a camera, I really can not find what to put in setpoint

10

One can take the readings from a mpu6050 accelerometer do a bit o math on the readings to get angle, like so

//Roll & Pitch Equations from accelerometers only
        float roll2  = (atan2(-(get_aY()), get_aZ()) * 180.0) / M_PI;
        float pitch2 = (atan2( get_aX(), sqrt(get_aY() * get_aY() + get_aZ() * get_aZ())) * 180.0) / M_PI;

and use the angle produced to generate a servo counter torque. One will find that accelermoeter angles alone are a bit shifty so one may want to calm them down by adding in a "arduino complementary filter" (do a search on those words to get a whole lot of thing do possibilities). Complementary filters worked well when I used them to help maintain a stable X/Y platform.

I would suspect torquing the servo in one degree increments may not be enough granularity so one can use micro seconds to torque the servo.
Let's say the range of the servo in uSeconds is 500 to 2500 which would give 2000uSeconds for 180 degrees, divided up and that is 11.11 uSeconds per degree. Now you can torque partial degrees.

If you want even more granularity you can use an ESP32 where clock ticks are used to torque the servo. It equates out to ~3 clock ticks = 1 uSec. Of course this granularity would be beyond the capability of the servo response range.

I, also, think that to use something more filtery, like a MahonyQuaternionUpdate on an Uno would be a bit much with all the floating point math needed.

11

What's the mechanical layout? It seems like you are only testing one axis for now. Keep the other one locked and approximately vertical at all times.

Is the sensor mounted to the servo?

12

Idahowalker:
One can take the readings from a mpu6050 accelerometer do a bit o math on the readings to get angle, like so [Complementary filters worked well when I used them to help maintain a stable X/Y platform.

I would suspect torquing the servo in one degree increments may not be enough granularity so one can use micro seconds to torque the servo.
Let's say the range of the servo in uSeconds is 500 to 2500 which would give 2000uSeconds for 180 degrees, divided up and that is 11.11 uSeconds per degree. Now you can torque partial degrees.

If you want even more granularity you can use an ESP32 where clock ticks are used to torque the servo. It equates out to ~3 clock ticks = 1 uSec. Of course this granularity would be beyond the capability of the servo response range.

I, also, think that to use something more filtery, like a MahonyQuaternionUpdate on an Uno would be a bit much with all the floating point math needed.

Okay thanks, for my angle i have

Gyr_rawX=Wire.read()<<8|Wire.read();
Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x; 
Gyro_angle_x = Gyr_rawX*elapsedTime;
.
.
.
 Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; 
  Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;
  Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ; 
Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg) ;
Total_angle_x = 0.98 *(Total_angle_x + Gyro_angle_x) + 0.02*Acc_angle_x; //Filter
[\code] 
Is this ok ? With "total_angle_x" when I test in practice it works properly.

But I do not understand how to modify the range of the servo ...

13

MorganS:
What's the mechanical layout? It seems like you are only testing one axis for now. Keep the other one locked and approximately vertical at all times.

Is the sensor mounted to the servo?

I put the sensor and the servo motor on the same plate like on this site https://www.instructables.com/id/Gyro-Stabilizer-W-Arduino-and-Servo/
but only with 1 servo motor for the moment

14

Lilou7812:
Okay thanks, for my angle i have
Is this ok ? With "total_angle_x" when I test in practice it works properly.

But I do not understand how to modify the range of the servo ...

if the x servo sits at 90 degrees for level and is tilted -1 degree what does the servo need to do to to come back to level?

15

I'm not sure to understand, the servo motor must go to 89 °?