#include <Wire.h>
#include "mpu.h"
#include "freeram.h"
#include <Servo.h>
#include <PID_v1.h>
#include "I2Cdev.h"
Servo sg90_x;
Servo sg90_y;
int servo_x = 5;
int servo_y = 6;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;
double Setpoint;
double x_in, x_out, y_in, y_out;
//PID parameters
double Kpx = 1, Kix = 0, Kdx = 0;
double Kpy = 1, Kiy = 10, Kdy = 0;
PID xPID(&x_in, &x_out, 0, Kpx, Kix, Kdx, DIRECT);
PID yPID(&y_in, &y_out, &Setpoint, Kpy, Kiy, Kdy, DIRECT);
int ret;
void setup ( )
{
Fastwire::setup(400,0);
Setpoint = 90;
sg90_x.attach (servo_x);
sg90_y.attach (servo_y);
sg90_x.write(Setpoint);
sg90_y.write(Setpoint);
ret = mympu_open(200);
xPID.SetMode(AUTOMATIC);
xPID.SetTunings(Kpx, Kix, Kdx);
xPID.SetOutputLimits(1000, 2000);
yPID.SetMode(AUTOMATIC);
yPID.SetTunings(Kpy, Kiy, Kdy);
delay (100);
}
void loop ( ){
ret = mympu_update();
x_in = mympu.ypr[1];
y_in = mympu.ypr[2];
xPID.Compute();
yPID.Compute();
sg90_x.writeMicroseconds(x_out);
sg90_y.writeMicroseconds(y_out);
}