#include <Wire.h>

#define CTRL_REG1 0x20

#define CTRL_REG2 0x21

#define CTRL_REG3 0x22

#define CTRL_REG4 0x23

#define CTRL_REG5 0x24

int L3G4200D_Address = 0x69;

float DPS_MULT = 0.0000085;

//int x = 0;

int y = 0;

//int z = 0;

//float angX = 0;

float angY = 0;

//float angZ = 0;

//float p_angX = 0;

float p_angY = 0;

//float p_angZ = 0;

//int gyroLowX = 0;

int gyroLowY = 0;

//int gyroLowZ = 0;

//int gyroHighX = 0;

int gyroHighY = 0;

//int gyroHighZ = 0;

unsigned long pastMicros = 0;

void setup()

{

Wire.begin();

Serial.begin(9600);

Serial.println("Starting up L3G4200D"); //vvvv

setupL3G4200D(250); // Configure L3G4200 - 250, 500 or 2000 deg/sec

delay(1000); // wait for the sensor to be ready 

calibrate();

attachInterrupt(0, updateAngle, RISING);

pastMicros = micros();

}

void loop()

{

getGyroValues(); 

float dt;

if(micros() > pastMicros) //micros() overflows every ~70 minutes

dt = (float) (micros()-pastMicros)/1000000.0;

else

dt = (float) ((4294967295-pastMicros)+micros())/1000000.0;

//if(x >= gyroHighX || x <= gyroLowX) {

//angX += ((p_angX + (x * DPS_MULT))/2) * dt;

//p_angX = x * DPS_MULT;

//} else {

//p_angX = 0;

//}

if(y >= gyroHighY || y <= gyroLowY) {

angY += ((p_angY + (y * DPS_MULT))/2) * dt;

p_angY = y * DPS_MULT;

} else {

p_angY = 0;

}

//if(z >= gyroHighZ || z <= gyroLowZ) {

//angZ += ((p_angZ + (z * DPS_MULT))/2) * dt;

//p_angZ = z * DPS_MULT;

//} else {

//p_angZ = 0;

//}

pastMicros = micros();

sendJson();

delay(10);

}

void updateAngle()

{

getGyroValues();

}

void calibrate()

{

Serial.println("Calibrating gyro, don't move!");

for(int i = 0; i < 200; i++) {

getGyroValues();

//if(x > gyroHighX)

//gyroHighX = x;

//else if(x < gyroLowX)

//gyroLowX = x;

if(y > gyroHighY)

gyroHighY = y;

else if(y < gyroLowY)

gyroLowY = y;

//if(z > gyroHighZ)

//gyroHighZ = z;

//else if(z < gyroLowZ)

//gyroLowZ = z;

delay(10);

}

Serial.println("Calibration complete.");

}

void sendJson() {

char json[40];

sprintf(json, "{\"y\":%d}", 

//(int)(angX*1000), 

(int)(angY*200) 

//(int)(angZ*1000)

);

Serial.println(json);

}

void getGyroValues() {

//byte xMSB = readRegister(L3G4200D_Address, 0x29);

//byte xLSB = readRegister(L3G4200D_Address, 0x28);

//x = ((xMSB << 8) | xLSB);

byte yMSB = readRegister(L3G4200D_Address, 0x2B);

byte yLSB = readRegister(L3G4200D_Address, 0x2A);

y = ((yMSB << 8) | yLSB);

//byte zMSB = readRegister(L3G4200D_Address, 0x2D);

//byte zLSB = readRegister(L3G4200D_Address, 0x2C);

//z = ((zMSB << 8) | zLSB);

}

int setupL3G4200D(int scale) {

writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

if(scale == 250) {

writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);

} else if(scale == 500) {

writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);

} else {

writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);

}

writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);

}

void writeRegister(int deviceAddress, byte address, byte val)

{

Wire.beginTransmission(deviceAddress); // start transmission to device 

Wire.write(address); // send register address

Wire.write(val); // send value to write

Wire.endTransmission(); // end transmission

}

int readRegister(int deviceAddress, byte address)

{

int v;

Wire.beginTransmission(deviceAddress);

Wire.write(address); // register to read

Wire.endTransmission();

Wire.requestFrom(deviceAddress, 1); // read a byte

while(!Wire.available()) {

// waiting

}

v = Wire.read();

return v;

}

아두이노에 자이로센서를 인식하게 하는 코딩입니다

저희는 여기서 

void sendJson() {

char json[40];

sprintf(json, "{\"y\":%d}", 

//(int)(angX*1000), 

(int)(angY*200) 

//(int)(angZ*1000)

);

센서의 y값만 인식되게 하려고 나머지 값을 주석처리하고 또 거기서 y값이 어떤 높이에서 시작하던 시작점이 0이 되고 움직이면 마이너스값이 되게하려면 어떻게 하나요? 지금 sprintf y에 -를 입력해서 값이 전부 마이너스가 나오긴한데 값이 자꾸 쌓이기만하고 0이 되지 않습니다. 시작점이 무조건 0이되고 움직일때 마이너스값이 될수있도록 코딩하는 방법 좀 가르쳐 주세요ㅠㅠㅠ