#include <Wire.h>
#include <Servo.h>
Servo myservo1;
Servo myservo2;
float OldValX=0;
float OldValY=0;
int val1=45, val2=45;
float x,y,z;
const int MPU_ADDR = 0x68;// 12C address of the MPU6050. If AD0 pin is set to HIGH, the 12C address will be 0x69
int16_t accelerometer_x, accelerometer_y, accelerometer_z;
//variables for accelerometer raw data
int16_t gyro_x,gyro_y, gyro_z;//variables for gyro raw data
//int16_t temperature;//variables for temperature data
char tmp_str[7];//temperory variabe used in convert function
char * convert_int16_to_str(int16_t i)
{//converts int 16 to string. Resulting strings will have same length in the debug monitor (Tools > Serial Monitor)
sprintf(tmp_str,"%6d",i);
return tmp_str;
}
void setup()
{
myservo1.attach(6);
myservo2.attach(7);
myservo1.write(45);
myservo2.write(45);
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_ADDR);//begins a transmission to the 12C slave(GY521 board)
Wire.write(0x6B);//PWR_MGMT_1 register
Wire.write(0);//set to 0(wakes up the MPU6050
Wire.endTransmission(true);
}
void loop()
{
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 6*2, true);
int accelerometer_x = Wire.read()<<8 | Wire.read();
int accelerometer_y = Wire.read()<<8 | Wire.read();
int accelerometer_z = Wire.read()<<8 | Wire.read();
//temperature = Wire.read()<<8 | Wire.read();
gyro_x = Wire.read()<<8 | Wire.read();
gyro_y = Wire.read()<<8 | Wire.read();
gyro_z = Wire.read()<<8 | Wire.read();
x = accelerometer_x/100;
y = accelerometer_y/100;
if(y>100){
y = 100;
}
if(y<-100){
y = -100;
}
if(x>100){
x = 100;
}
if(x<-100){
x = -100;
}
//print out data
//serial.print("aX = ");
Serial.print(x);
//serial.print(" | aY = ");
Serial.print("\t");
Serial.print(y);
Serial.print("\t");
//serial.print(" | aY = ");
Serial.print(val1);
Serial.print("\t");
Serial.println(val2);
val1=map(x,115, -115, 0, 90);
val2=map(y,115, -115, 0, 90);
myservo1.write(val1);
myservo2.write(val2);
delay(150);
}