fulstabil.ino

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.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;
float deadzone;

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

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;
  deadzone = 50;

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

void loop(){
/*
  while(Serial.available()) {
    unsigned char b = Serial.read()
    if (b == 't') {
      unsigned char b = Serial.read()
    }
    if (b == 'f') {
      unsigned char b = Serial.read()
    }
  }
*/

  
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,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)
/*  
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
*/
  
  cur_y = AcY; // accel_t_gyro.value.y_accel;

//  if (cur_y > (up_y - deadzone)) { cur_y = up_y; }
//  if (cur_y < (up_y + deadzone)) { cur_y = up_y; }

  avg_y = (avg_y * 0.7 + cur_y * 0.3);

  int mot_y = (up_y - avg_y) / 40;

  Serial.print("avg:");
  Serial.print(floor(avg_y));
  Serial.print(" motor:");
  Serial.print(floor(mot_y));
  Serial.print("\n");

  if (mot_y > 0) {
    int x = mot_y;
    if (x > 255) x = 255;
    if (x < 0) x = 0;
    analogWrite(6, x);
    digitalWrite(3, LOW);
    analogWrite(10, x);
    digitalWrite(9, LOW);
    delay(10);
    analogWrite(6, 0);
    digitalWrite(3, LOW);
    analogWrite(10, 0);
    digitalWrite(9, LOW);
  }

  if (mot_y < 0) {
    int x = -mot_y;
    if (x > 255) x = 255;
    if (x < 0) x = 0;
    digitalWrite(6, LOW);
    analogWrite(3, x);
    digitalWrite(10, LOW);
    analogWrite(9, x);
    delay(10);
    digitalWrite(6, LOW);
    analogWrite(3, 0);
    digitalWrite(10, LOW);
    analogWrite(9, 0);
  }
   
  delay(2);
}