Arduino code 2: Difference between revisions

From Control Systems Technology Group
Jump to navigation Jump to search
(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_…')
 
No edit summary
 
(One intermediate revision by the same user not shown)
Line 1: Line 1:
<pre>
<pre>
#include <Wire.h>
#include "freeram.h"
#include "mpu.h"
#include "mpu.h"
#include "freeram.h"
#include "I2Cdev.h"
#include <Servo.h>
#include <PID_v1.h>
#include <PID_v1.h>
#include "I2Cdev.h"
#include <Servo.h>


Servo sg90_x;
Servo sg90_x; //sg90 is the type of servo we are using
Servo sg90_y;
Servo sg90_y;


int servo_x = 5;
int servo_x = 9;
int servo_y = 6;
int servo_y = 10;


int16_t ax, ay, az ;
double x_in, x_out, y_in, y_out;
int16_t gx, gy, gz ;


double Setpoint;
//PID parameters, these are tuned exactly for our device. A different sensor or different servo's may need different tuning parameters
double x_in, x_out, y_in, y_out;
double Kpx = 0.1, Kix = 0.05, Kdx = 0.25;
//PID parameters
double Kpy = 0.75, Kiy = 0.10, Kdy = 0.34;
double Kpx = 1, Kix = 0, Kdx = 0;
 
double Kpy = 1, Kiy = 10, Kdy = 0;  
PID xPID(&x_in, &x_out, 1500, Kpx, Kix, Kdx, REVERSE);
PID yPID(&y_in, &y_out, 1500, Kpy, Kiy, Kdy, DIRECT);


PID xPID(&x_in, &x_out, 0, Kpx, Kix, Kdx, DIRECT);
PID yPID(&y_in, &y_out, &Setpoint, Kpy, Kiy, Kdy, DIRECT);
int ret;
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());


void setup ( )
    sg90_x.attach(servo_x);
{
    sg90_y.attach(servo_y);
  Fastwire::setup(400,0);
   
  Setpoint = 90;
    sg90_x.write(90); //set the servo's to their middle positions.
  sg90_x.attach (servo_x);
    sg90_y.write(90);
  sg90_y.attach (servo_y);
   
  sg90_x.write(Setpoint);
    xPID.SetMode(AUTOMATIC);
  sg90_y.write(Setpoint);
    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.
  }


   ret = mympu_open(200);
void loop() {
xPID.SetMode(AUTOMATIC);
   ret = mympu_update();
xPID.SetTunings(Kpx, Kix, Kdx);
  x_in = mympu.ypr[1]; //in Arduino an array starts with zero, therefore the value of the yaw axis is retrieved with ypr[0]
xPID.SetOutputLimits(1000, 2000);
  y_in = mympu.ypr[2];// the pitch axis with ypr[1] and the roll axis with ypr[2]
yPID.SetMode(AUTOMATIC);
 
yPID.SetTunings(Kpy, Kiy, Kdy);
  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();
    
    
   delay (100);
   sg90_x.write(x_out);
}
  sg90_y.write(y_out));
 
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);
 
 
 
 
}
}
</pre>
</pre>

Latest revision as of 20:22, 5 April 2020

#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));
}