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