Aplikasi Accelerometer ADXL345 mengendalikan 2 motorservo

Video ini memperlihatkan aplikasi Accelerometer ADXL345 untuk mengatur gerakan 2 buah servo motor (sumbu x dan sumbu y)

 

Schematic:

List Program

// ADXL345 control 2 Servo Motors
// Modified by: Aan Darmawan
// valfa.blogspot.com
// June 2011
//
#include <Wire.h>
#include <Servo.h> 
Servo myservox; // servo for x axis
Servo myservoy; // servo for y axis
#define DEVICE (0x53) //ADXL345 device address
#define TO_READ (6)   //num of bytes we are going to read each time (two bytes for each axis)
#define TRIGGER 16    // pin analog2 (16) connect to CS Adxl345, active high
byte buff[TO_READ] ;  //6 bytes buffer for saving data read from the device
int i;

void setup()
{
  pinMode(TRIGGER,OUTPUT);
  digitalWrite(TRIGGER,HIGH);
  Wire.begin();        // join i2c bus (address optional for master)
  myservox.attach(9);  // attaches the servo on pin 9 to the servo object 
  myservoy.attach(10); // attaches the servo on pin 10 to the servo object 
  myservox.write(90);  // tell servo x to go to position 90 deg
  delay(25);           // makesure servo move
  myservoy.write(90);  // tell servo y to go to position 90 deg
  delay(25);          // makesure servo move
  //Turning on the ADXL345
  writeTo(DEVICE, 0x2D, 0);      
  writeTo(DEVICE, 0x2D, 16);
  writeTo(DEVICE, 0x2D, 8);
}

void loop()
{
  int regAddress = 0x32;    //first axis-acceleration-data register on the ADXL345
  int x, y, z;
  digitalWrite(TRIGGER,LOW);
  delay(10);
  digitalWrite(TRIGGER,HIGH); 
  readFrom(DEVICE, regAddress, TO_READ, buff); //read the acceleration data from the ADXL345

  //each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
  //thus we are converting both bytes in to one int
  //read 5 times and average them
  x=0;
  y=0;
  z=0;
  for(i=1;i<=5;i++){
    x += (((int)buff[1]) << 8) | buff[0];   
    y += (((int)buff[3])<< 8) | buff[2];
    z += (((int)buff[5]) << 8) | buff[4];
    delay(10);
  }
  x/=5;
  y/=5;
  z/=5;
  if(x<-255)x= -255; else if (x>255)x=255;
  if(y<-255)y= -255; else if (y>255)y=255;
  x=map(x, -255, 255, 0, 180);  // map range y
  y=map(y, -255, 255, 0, 180);  // map range y
  myservox.write(x);            // tell servo to go to position in variable x
  delay(25); 
  myservoy.write(180-y);            // tell servo to go to position in variable y 
  delay(25); 
   //It appears that delay is needed in order not to clog the port
  delay(200);
}

//---------------- Functions
//Writes val to address register on device
void writeTo(int device, byte address, byte val) {
  Wire.beginTransmission(device); //start transmission to device 
  Wire.send(address);        // send register address
  Wire.send(val);        // send value to write
  Wire.endTransmission(); //end transmission
}

//reads num bytes starting from address register on device in to buff array
void readFrom(int device, byte address, int num, byte buff[]) {
  Wire.beginTransmission(device); //start transmission to device 
  Wire.send(address);        //sends address to read from
  Wire.endTransmission(); //end transmission

    Wire.beginTransmission(device); //start transmission to device
  Wire.requestFrom(device, num);    // request 6 bytes from device

  int i = 0;
  while(Wire.available())    //device may send less than requested (abnormal)
  { 
    buff[i] = Wire.receive(); // receive a byte
    i++;
  }
  Wire.endTransmission(); //end transmission
}