Created
April 14, 2015 16:04
-
-
Save sezgingul/48ba7ea8c4390395365c to your computer and use it in GitHub Desktop.
3 servo control
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// ADXL345 ivmeölçer 3 Servo Motor kontrolü | |
#include <Wire.h> | |
#include <Servo.h> | |
Servo myservox; | |
Servo myservoy; | |
Servo myservoz; | |
#define DEVICE (0x53) | |
#define TO_READ (6) | |
#define TRIGGER 16 | |
byte buff[TO_READ] ; | |
int i; | |
void setup() | |
{ | |
pinMode(TRIGGER,OUTPUT); | |
digitalWrite(TRIGGER,HIGH); | |
Wire.begin(); | |
myservox.attach(9); | |
myservoy.attach(10); | |
myservoy.attach(11); | |
myservox.write(90); | |
delay(25); | |
myservoy.write(90); | |
delay(25); | |
myservoz.write(90); | |
delay(25); | |
writeTo(DEVICE, 0x2D, 0); | |
writeTo(DEVICE, 0x2D, 16); | |
writeTo(DEVICE, 0x2D, 8); | |
} | |
void loop() | |
{ | |
int regAddress = 0x32; | |
int x, y, z; | |
digitalWrite(TRIGGER,LOW); | |
delay(10); | |
digitalWrite(TRIGGER,HIGH); | |
readFrom(DEVICE, regAddress, TO_READ, buff); | |
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; | |
if(z<-255)z= -255; else if (z>255)z=255; | |
x=map(x, -255, 255, 0, 180); | |
y=map(y, -255, 255, 0, 180); | |
z=map(z, -255, 255, 0, 180); | |
myservox.write(x); | |
delay(25); | |
myservoy.write(180-y); | |
delay(25); | |
myservoz.write(180-z); | |
delay(25); | |
delay(200); | |
} | |
void writeTo(int device, byte address, byte val) { | |
Wire.beginTransmission(device); | |
Wire.write(address); | |
Wire.write(val); | |
Wire.endTransmission(); | |
} | |
void readFrom(int device, byte address, int num, byte buff[]) { | |
Wire.beginTransmission(device); | |
Wire.write(address); | |
Wire.endTransmission(); | |
Wire.beginTransmission(device); | |
Wire.requestFrom(device, num); | |
int i = 0; | |
while(Wire.available()) | |
{ | |
buff[i] = Wire.read(); | |
i++; | |
} | |
Wire.endTransmission(); | |
} | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment