Help PID control with Gyro

Hi, I am current working on a project which I am gonna built a 2-D self stabilization platform with 2 servos. At the beginning, I just used the value directly from what I have got from gyroscope to control the servos. Then I found out that the platform is shaky, I did a little bit research on this, resulting in PID controller probably is a good alternative for me. I have attached my code to this post. Anyone here is familiar with the PID programming, could you please take a look at this code and help me to figure out how to include the PID to it? It would be greatly appreciated.

#include <Servo.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
Servo myservo;
Servo myservoy;
int val;
int valy;
int prevVal;
int prevValy;

void setup()
Serial.println(‘Initialize MPU’);
Serial.println(mpu.testConnection() ? ‘Connected’ : ‘Connection failed’);
void loop()
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

valy = map(ay, -17000, 17000, 0, 179);
val = map(ax, -17000, 17000, 0, 179);

if (val != prevVal)
prevVal = val;
Serial.print("x : ");

if (valy != prevValy)
prevValy = valy;
Serial.print("y : ");



I would suggest downloading the PID library from github. Then look at the Basic sketch included. It shows you the fundamentals. I am no expert on PID... I am just like you, trying to find out more for use on my project (Tricopter). You need to map the IMU values into something the PID can control and output to servo. For example, when the IMU is completely level, the mapped value could be 90.

Hope this helps.


myservoy.attach( 8);

A perfect example of why Read this before posting a programming question ... - Programming Questions - Arduino Forum advises you to use code tags.