code :
#include <Servo.h>
boolean x=false;
Servo servo1,servo2;
#define CD4053_C 10
#define CD4053_B 9
#define CD4053_A 8
//PIN
#define PIN_SDA 20
#define PIN_SCL 21
//I2C (sparkfun breakout)
#define Register_ID 0
#define Register_2D 0x2D
#define Register_X0 0x32
#define Register_X1 0x33
#define Register_Y0 0x34
#define Register_Y1 0x35
#define Register_Z0 0x36
#define Register_Z1 0x37
#include <Wire.h>
int ADXAddress = 0x53; // the default 7-bit slave address
int reading = 0;
int val=0;
int X0,X1,X_out;
int Y0,Y1,Y_out;
int Z1,Z0,Z_out;
double Xg,Yg,Zg;
int pos1,pos2;
char flag=1;
void Wire_Start(){
Wire.beginTransmission(ADXAddress);
Wire.write(Register_2D);
Wire.write(8); //measuring enable
Wire.endTransmission(); // stop transmitting
}
void Wire_Get(){
//--------------X
Wire.beginTransmission(ADXAddress); // transmit to device
Wire.write(Register_X0);
Wire.write(Register_X1);
Wire.endTransmission();
Wire.requestFrom(ADXAddress,2);
if(Wire.available()<=2)
{
X0 = Wire.read();
X1 = Wire.read();
X1=X1<<8;
X_out=X0+X1;
}
//------------------Y
Wire.beginTransmission(ADXAddress); // transmit to device
Wire.write(Register_Y0);
Wire.write(Register_Y1);
Wire.endTransmission();
Wire.requestFrom(ADXAddress,2);
if(Wire.available()<=2)
{
Y0 = Wire.read();
Y1 = Wire.read();
Y1=Y1<<8;
Y_out=Y0+Y1;
}
//------------------Z
Wire.beginTransmission(ADXAddress); // transmit to device
Wire.write(Register_Z0);
Wire.write(Register_Z1);
Wire.endTransmission();
Wire.requestFrom(ADXAddress,2);
if(Wire.available()<=2)
{
Z0 = Wire.read();
Z1 = Wire.read();
Z1=Z1<<8;
Z_out=Z0+Z1;
}
//----------------
Xg=X_out/256.0;
Yg=Y_out/256.0;
Zg=Z_out/256.0;
}
void setup()
{
Wire.begin();
if(x)
Serial.begin(9600);
delay(100);
pinMode(CD4053_A,OUTPUT);
pinMode(CD4053_B,OUTPUT);
pinMode(CD4053_C,OUTPUT);
// enable to measute g data
servo2.write(175);
servo2.attach(47);
servo1.write(175);
servo1.attach(49);
pos1=servo1.read();
pos2=servo2.read();
if(x){
Serial.print("-->Start: pos1:");
Serial.print(pos1);
Serial.print(" pos2:");
Serial.println(pos2);}
digitalWrite(CD4053_A,HIGH);
digitalWrite(CD4053_B,LOW);
digitalWrite(CD4053_C,LOW);
delay(100);
Wire_Start();
digitalWrite(CD4053_A,LOW);
digitalWrite(CD4053_B,HIGH);
delay(100);
Wire_Start();
}
void loop()
{
if(x){
Serial.print("----");
Serial.print((flag+0));
Serial.println("----");}
switch(flag){
case 1: {
pos1=servo1.read();
if(x)
Serial.println(pos1);
digitalWrite(CD4053_A,HIGH);
digitalWrite(CD4053_B,LOW);
// digitalWrite(CD4053_C,LOW);
Wire_Get();
if(Xg>0)
pos1=90+(Xg<1?85Xg:85);
else
pos1=90+(Xg>-1?85Xg:-85);
servo1.write(pos1);
flag=2;
break;
}
case 2:{
pos2=servo2.read();
if(x)
Serial.println(pos2);
digitalWrite(CD4053_B,HIGH);
digitalWrite(CD4053_A,LOW);
// digitalWrite(CD4053_C,LOW);
Wire_Get();
if(Xg>0)
pos2=90+(Xg<1?85Xg:85);
else
pos2=90+(Xg>-1?85Xg:-85);
servo2.write(pos2);
flag=1;
break;
}
}
if(x){
Serial.print("X= ");
Serial.print(Xg);
Serial.print(" ");
Serial.print("Y= ");
Serial.print(Yg);
Serial.print(" ");
Serial.print("Z= ");
Serial.println(Zg);}
delay(1000);
}