Arduino code 2

From Control Systems Technology Group
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));
}