#include "freeram.h"
#include "mpu.h"
#include "I2Cdev.h"
#include <PID_v1.h>
#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));
}