Arduino code 2
Jump to navigation
Jump to search
#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)); }