fulstabil2.ino

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
#include <PID_v1.h>

const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

float avg_y;
float cur_y;
float tar_y;
float up_y;

int dx = 128;
int dy = 128;

int motor_counter1;
int motor_counter2;
int motor_counter3;
int motor_counter4;

double Setpoint, Input, Output;
// double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.1, consKd=0.01;

double leftSetpoint, leftInput, leftOutput;
double rightSetpoint, rightInput, rightOutput;

PID leftPid(&leftInput, &leftOutput, &leftSetpoint, consKp, consKi, consKd, DIRECT);
PID rightPid(&leftInput, &rightOutput, &rightSetpoint, consKp, consKi, consKd, DIRECT);

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(57600);

  pinMode(3, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);

  avg_y = -1;
  up_y = 928;
//  up_y = 50;

  motor_counter1 = 0;
  motor_counter2 = 0;
  motor_counter3 = 0;
  motor_counter4 = 0;


  leftPid.SetMode(AUTOMATIC);
  leftPid.SetOutputLimits(-255, 255);

  rightPid.SetMode(AUTOMATIC);
  rightPid.SetOutputLimits(-255, 255);
}


void loop(){
  while(Serial.available()) {
    unsigned char b1 = Serial.read();
    if (b1 == 'x') {
      unsigned char b2 = Serial.read();
      dx = b2;
    }
    if (b1 == 'y') {
      unsigned char b2 = Serial.read();
      dy = b2;
    }
  }
  
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,6,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  // Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  // GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  // GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  // GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  
  /*
  cur_y = AcY;
  avg_y = (avg_y * 0.5 + cur_y * 0.5);
  */
  int new_up_y1 = up_y + (dy - 128) * 50 + (dx - 128) * 50;
  int new_up_y2 = up_y + (dy - 128) * 50 + (128 - dx) * 50;
  /*
  int mot_y1 = (new_up_y1 - avg_y) / 40;
  int mot_y2 = (new_up_y2 - avg_y) / 40;
  */
  leftSetpoint = new_up_y1 / 150.0;
  rightSetpoint = new_up_y2 / 150.0;

  leftInput = AcY / 150.0;
  rightInput = AcY / 150.0;
  
  leftPid.Compute();
  rightPid.Compute();

  int mot_y1 = leftOutput;
  int mot_y2 = rightOutput;

  Serial.print("dx:");
  Serial.print(floor(dx));
  Serial.print("  dy:");
  Serial.print(floor(dy));
  Serial.print("  in:");
  Serial.print(floor(leftInput));
  Serial.print("  set:");
  Serial.print(floor(leftSetpoint));
  Serial.print("  motor1:");
  Serial.print(floor(mot_y1));
  Serial.print("  motor2:");
  Serial.print(floor(mot_y2));
  Serial.print("\n");

  if (mot_y1 >= 0) {
    int x = mot_y1;
    if (x > 255) x = 255;
    if (x < 0) x = 0;
    analogWrite(6, x);
    digitalWrite(3, LOW);
  //  delay(8);
  //  analogWrite(6, 0);
  //  digitalWrite(3, LOW);
  }

  if (mot_y1 < 0) {
    int x = -mot_y1;
    if (x > 255) x = 255;
    if (x < 0) x = 0;
    digitalWrite(6, LOW);
    analogWrite(3, x);
  //  delay(8);
  //  digitalWrite(6, LOW);
  //  analogWrite(3, 0);
  }
   
  if (mot_y2 >= 0) {
    int x = mot_y2;
    if (x > 255) x = 255;
    if (x < 0) x = 0;
    analogWrite(10, x);
    digitalWrite(9, LOW);
  //  delay(8);
  //  analogWrite(10, 0);
  //  digitalWrite(9, LOW);
  }

  if (mot_y2 < 0) {
    int x = -mot_y2;
    if (x > 255) x = 255;
    if (x < 0) x = 0;
    digitalWrite(10, LOW);
    analogWrite(9, x);
  //  delay(8);
  //  digitalWrite(10, LOW);
  //  analogWrite(9, 0);
  }
   
  delay(1);
}