Arduino code 2

From Control Systems Technology Group
Revision as of 14:55, 28 March 2020 by H.e.h.hanegraaf@student.tue.nl (talk | contribs) (Created page with '<pre> #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_…')
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
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);




}