Arduino code 2

1. include "freeram.h"
2. include "mpu.h"
3. include "I2Cdev.h"
4. include <PID_v1.h>
5. include <Servo.h>

Servo sg90_x; //sg90 is the type of servo we are using Servo sg90_y;

int servo_x = 9; int servo_y = 10;

double x_in, x_out, y_in, y_out;

//PID parameters, these are tuned exactly for our device. A different sensor or different servo's may need different tuning parameters double Kpx = 0.1, Kix = 0.05, Kdx = 0.25; double Kpy = 0.75, Kiy = 0.10, Kdy = 0.34;

PID xPID(&x_in, &x_out, 1500, Kpx, Kix, Kdx, REVERSE); PID yPID(&y_in, &y_out, 1500, Kpy, Kiy, Kdy, DIRECT);

int ret; void setup() {

   Fastwire::setup(400,0);
   Serial.begin(115200);
   ret = mympu_open(200);
   Serial.print("MPU init: "); Serial.println(ret);
   Serial.print("Free mem: "); Serial.println(freeRam());

   sg90_x.attach(servo_x);
   sg90_y.attach(servo_y);
   
   sg90_x.write(90); //set the servo's to their middle positions. 
   sg90_y.write(90);
   
   xPID.SetMode(AUTOMATIC);
   xPID.SetTunings(Kpx, Kix, Kdx);
   yPID.SetMode(AUTOMATIC);
   yPID.SetTunings(Kpy, Kiy, Kdy);
   
   xPID.SetOutputLimits(0, 180); //the PID functions are by default programmed to output somewhere in between 0 and 255, but since 
   yPID.SetOutputLimits(0, 180);// the servo's operate between 0 and 180 degrees, the output is limited. 
 }

void loop() {

 ret = mympu_update();
 x_in = mympu.ypr[1]; //in Arduino an array starts with zero, therefore the value of the yaw axis is retrieved with ypr[0]
 y_in = mympu.ypr[2];// the pitch axis with ypr[1] and the roll axis with ypr[2]
 
 x_in = map(x_in, -90, 90, 0, 180); //map the values within operating range before the PID controller computes the output values
 y_in = map(x_in, -90, 90, 0, 180);
 
 xPID.Compute();
 yPID.Compute();
 
 sg90_x.write(x_out);
 sg90_y.write(y_out));

}