Contoh Aplikasi Accelerometer ADXL345 mengendalikan 2 motorservo

Dibawah ini memperlihatkan aplikasi Accelerometer ADXL345 untuk mengatur gerakan 2 buah servo motor (sumbu x dan sumbu y). aplikasi ini yang saya pake di projek saya dengan mengunakan arduino.

Schematic:

List Program

// ADXL345 control 2 Servo Motors

#include <Wire.h>
#include <Servo.h>

Servo myservox; // servo for x axis
Servo myservoy; // servo for y axis

#define DEVICE (0×53)  //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 = 0×32;    //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
}

Post a Comment