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;

}

Oh, so I’m supposed to be holding the top whilst it oscillates. Thanks! That makes more sense now. I was worried it meant that the robot needed to self balance just to look at the oscillation. :)

]]>“Oscillating” means that the robot moves about its vertical axis. The best way to imagine this is that suppose it is in equilibrium, and if you push the top forward a little (in other words, it starts to fall forward) you will see that the wheels move forward accordingly and if you move the top backward, the wheels will move backward to maintain balance.

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.

]]>The INIT_ANGLE is the angle reading when the robot is in balance (ideally it should be 0 but this value was first measured to ensure that it can reach the balanced stage)

]]>oppps … in later reading i got it :)

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.

]]>hmm.. i erased the functions of getAccXYZ() and VxGyro = analogRead(PIN_GYRO_X); (Just keep them reference)

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..

]]>Hmm… doesn’t look quite right. If the sensor did not move, the gyroscope anagle should be near 0 and the calculated angle average should be near constant.

]]>for this case, I just kept MPU-9150 stay on my desk.

]]>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