// MCU BASIC: https://www.basic4mcu.com
// DateTime : 2018-06-24 오후 4:11:28
// by Ok-Hyun Park
아두이노 우노 R3과 mpu6050센서 조이스틱으로
2개의서보모터를 제어 하고자 합니다
지금의 코드로는 mpu6050센서값으로 서보모터가 잘 움직입니다
하지만 조이스틱으로 제어하고자 할때 계속 자이로센서를 따라가버려서 원하는 위치로 가질 않고 있습니다.
//
조이스틱으로 제어시 서보모터가 자이로센서에 영향을 받지 않고 제어되다가
조이스틱의 움직임이 없을시 다시 자이로센서로 수평을 유지 하고자 합니다
mpu6050에 슬립모드라는게 있다는걸 알게 되었는데 어떻게 적용해야 할지 막막합니다
//
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;
#include "Servo.h"
Servo servo1;
Servo servo2;
//
int16_t ax,ay,az;
int16_t gx,gy,gz;
int angle1=90;
int val1,val2;
int prevVal1,prevVal2;
//
void setup(){
  Wire.begin();
  Serial.begin(38400);
  Serial.println("Initialize MPU");
  mpu.initialize();
  Serial.println(mpu.testConnection()?"Connected": "Connection failed");
  servo1.attach(9); servo1.write(90);
  servo2.attach(10); servo2.write(90);
}
//
void loop(){
  int X=analogRead(A0);
  int Y=analogRead(A1);
  if((X>=400)&&(X<=600)&&(Y>=400)&&(Y<=600)){
    mpu.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
    val1=map(ax,-17000,17000,0,179);
    if(val1!=prevVal1){ servo1.write(val1); prevVal1=val1; }
    Serial.print("X_AXIS: "); Serial.println(X);
    Serial.print("Y_AXIS: "); Serial.println(Y);
    Serial.print("Filter angle: "); Serial.println(val1);
    val2=map(ay,-17000,17000,0,179);
    if(val2!=prevVal2){ servo2.write(val1); prevVal1=val1; }
  }
  else{
    if(X<400){
      if(angle1<180){ servo1.write(angle1++); delay(50); }
      else if(angle1==180){ /*balnk*/}
    }
    else if(X>600){
      if(angle1>0){ servo1.write(angle1--); delay(50); }
      else if(angle1==0){ /*b; ank*/}
    }
    //
    if(Y<400){
      if(angle1<180){ servo2.write(angle1++); delay(50); }
      else if(angle1==180){ /*balnk*/}
    }
    else if(Y>600){
      if(angle1>0){ servo2.write(angle1--); delay(50); }
      else if(angle1==0){ /*b; ank*/}
    }
  }
  //
  delay(100);
}