@osmaneralp
I had something simulare but i found that I had the wrong value when setting the scaling of the gyro rate. Check in the data sheet for you sensor what the sensitivity is for mV/(deg/sec)
int getGyroRate() { // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
return int(sensorValue[GYR_Y] * 4.583333333); // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}
Also it's important that you have a good value on the ACC_Z and it should be equal when having it standing as up side down...
For ACC_Z, gravity correction = (0° value - 180° value)/2
2 – Zeroing sensor
Before aquiring data in the loop, sensors should be zeroed
This means that when the bot is stricly still and vertical, sensors should read “0[ch8243]
except for the vertical axis (Acc_Z), which is sensing gravity (1g)
Zero mesurement is performed in setup:void calibrateSensors() { // Set zero sensor values
long v;
for(int n=0; n<3; n++) {
v = 0;
for(int i=0; i<50; i++) v += readSensor(n);
sensorZero[n] = v/50;
}
sensorZero[ACC_Z] -= 102;
}
calibrateSensors() is a one off action, so we have time to average 3 X 50 measurements Finally, gravity value is deducted to Acc_Z For ADXL330/335: 1g = 330 mV (+- 10%) ACC_Z correction for 10 bit ADC and 3.3V AREF: 330/3300*1024 = 102 (to be fine tuned later on)
Hope you find the problem.. What sensor are you using?