TO calculate theta in degrees you use the equation:

theta = [Vgyro * 3300 * 2 * delta(t)]/ [(2^10) *180]

Shouldn’t it be:

theta = [Vgyro * 3300 * PI * delta(t)]/ [((2^10)-1) *180 *2]

I don’t understand why.

Could you please explain?

void getAccXYZ(int *x, int *y, int *z, bool isHighRes = true) {

byte buf[6];

if (isHighRes) {

regRead(REG_OUT_X_MSB, buf, 6);

*x = buf[0] <> 6 & 0x3;

*y = buf[2] <> 6 & 0x3;

*z = buf[4] <> 6 & 0x3;

} else {

regRead(REG_OUT_X_MSB, buf, 3);

*x = buf[0] << 2;

*y = buf[1] << 2;

*z = buf[2] < 511) *x = *x – 1024;

if (*y > 511) *y = *y – 1024;

if (*z > 511) *z = *z – 1024;

}

In practice, since you can’t place the robot at its exact balancing point, the movement will be a slight back and forth (while you are holding the top, that is), basically, the robot will over-correct a little bit and go back and forth ever so slightly.

]]>sorry for that.

]]>i was trying to learn your code. but i got stuck .. where / what is the

INIT_ANGLE came from ?

not declared .. and not clear.

can you please explain ?

void loop() {

float a = EstimateAngle() – INIT_ANGLE;

thank you.

]]>and i inserted the function of getting Acc value and Gyro value, accelgyro.getMotion9(&VxAcc, &VyAcc, &VzAcc, &VxGyro, &VyGyro, &VzGyro);, in the function of GetAccAngle()

but It doesn’t get right values.. i don’t know why..

]]>I applied your code for my peripherals. I used Arduino UNO and MPU-9150. So I tested code for this.

Is it a right result?

regards,

Alex

GyroX Stationary=0.00

currAngleAvg=0.0000 currAngle=-4.2273 accAngle=0.99 gyroAngle=-4.50 VxGyro=-158 elapsedTime=795720 throttle=0.00

-132013.96

currAngleAvg=-4.2223 currAngle=-4.5467 accAngle=0.99 gyroAngle=-0.61 VxGyro=-194 elapsedTime=87908 throttle=132013.96

-26317.73

currAngleAvg=-4.3820 currAngle=-4.9003 accAngle=0.99 gyroAngle=-0.66 VxGyro=-136 elapsedTime=136240 throttle=26317.73

-34081.08

currAngleAvg=-4.5531 currAngle=-5.4866 accAngle=0.99 gyroAngle=-0.93 VxGyro=-190 elapsedTime=136248 throttle=34081.08

-35341.52

currAngleAvg=-4.9729 currAngle=-6.2751 accAngle=0.99 gyroAngle=-1.17 VxGyro=-240 elapsedTime=136232 throttle=35341.52

-37067.46

currAngleAvg=-5.5490 currAngle=-7.1354 accAngle=0.99 gyroAngle=-1.29 VxGyro=-264 elapsedTime=136240 throttle=37067.46

-39303.75

currAngleAvg=-6.2940 currAngle=-7.6932 accAngle=0.99 gyroAngle=-1.01 VxGyro=-208 elapsedTime=136240 throttle=39303.75

-41510.41

currAngleAvg=-7.0296 currAngle=-7.8848 accAngle=0.99 gyroAngle=-0.66 VxGyro=-135 elapsedTime=136240 throttle=41510.41

-43120.18

currAngleAvg=-7.5662 currAngle=-8.0715 accAngle=0.99 gyroAngle=-0.66 VxGyro=-136 elapsedTime=136240 throttle=43120.18

-44056.21

currAngleAvg=-7.8782 currAngle=-8.5686 accAngle=0.99 gyroAngle=-1.00 VxGyro=-205 elapsedTime=136240 throttle=44056.21

-44931.57

currAngleAvg=-8.1700 currAngle=-9.1938 accAngle=0.99 gyroAngle=-1.16 VxGyro=-238 elapsedTime=136240 throttle=44931.57

-46240.54

currAngleAvg=-8.6063 currAngle=-9.5885 accAngle=0.99 gyroAngle=-0.95 VxGyro=-195 elapsedTime=136240 throttle=46240.54

-47757.53

currAngleAvg=-9.1119 currAngle=-10.1951 accAngle=0.99 gyroAngle=-1.20 VxGyro=-245 elapsedTime=136240 throttle=47757.53

-49384.04

currAngleAvg=-9.6541 currAngle=-10.4252 accAngle=0.99 gyroAngle=-0.83 VxGyro=-169 elapsedTime=137280 throttle=49384.04