A Self-Balancing Robot – III

In part one, I went over the theory behind the self-balancing robot and in part two, I showed you my build using a modified toy truck. In this post, I will walk you through the code and show you the finished balancing robot. If you are impatient, you can scroll to the end for the demostration video and the source code.

The code uses the Arduino framework and is built using the Netbeans IDE. For tutorials on how to develop Arduino applications using Netbeans, please take a look at my previous posting here. If you prefer the standard Arduino IDE, you may need to change the include statements to reflect your Arduino library path setup. The entire code listing can be downloaded here. Now let’s move on to the code.

Gyroscope calibration

LPY450AL is an analog gyroscope and it is fairly easy to interface with the MCU using ATMega328’s analog inputs. Before performing any measurements, the gyroscope needs to be calibrated so we know where the zero point is (i.e. when there is no rotation). To do this, the following CalGyro routine is called right upon powering up within setup().

In order to ensure accuracy, the platform must be stationary as any movement will likely affect the zero position readings of the gyroscope. As you can see, the code basically reads and averages the axis’ outputs while the robot is stationary and thus the raw ADC voltage value obtained represents the gyroscope output value when there is no rotation. The readings are averaged over 25 iterations to minimize errors. An LED controlled via PIN_MODE_LED is used to indicate whether the robot is undergoing the calibration process. So it is important for the robot to be stationary when the mode LED is lit, otherwise the calibrated value would be inaccurate.

float CalGyro() {
    float val = 0.0;
    digitalWrite(PIN_MODE_LED, HIGH);
    delay(500);
    for (int i = 0; i < 25; i++) {
        val += analogRead(PIN_GYRO_X);
        delay(10);
    }

    val /= 25.0;

    Serial.print("GyroX Stationary=");
    Serial.println(val);
    digitalWrite(PIN_MODE_LED, LOW);

    return val;
}

Inclination angle estimation

The following routine reads the current angle measurement from the accelerometer. I discussed the code for MMA8453Q in one of my previous post. So if you wanted to learn a bit more about this accelerometer, you can take a look there.

If you recall from our discussion in the first part, the inclination angle measured by the accelerometer is given by:
\[\theta\approx sin(\theta)=\frac{A_x}{g} \quad \text{… (1)}\]

Where Ax is the accelerometer x axis reading. In the implementation below, the axis used was slightly different due to the placement and the orientation of the accelerometer, but the calculation is the same:

float GetAccAngle() {
    getAccXYZ(&VxAcc, &VzAcc, &VyAcc);

    float r = sqrt((float) VzAcc * (float) VzAcc + (float) VyAcc * (float) VyAcc);
    accAngle = (float) VzAcc / r; //approximates sine

    return accAngle;
}

The angle estimation routine using the IMU (both accelerometer and gyroscope) is pretty straight forward as well. If you recall from our previous discussion, we used the following equations to calculate the estimated inclination angle in radians using the measurements from both the accelerometer and the gyroscpe:

\[\theta(t) \approx \theta(t_1) + G(t)(t_2 – t_1) = \theta(t_1) + G(t)\Delta t \quad \text{… (2)}\]

\[\left\{
\begin{array}{l l}\theta_{est}\mid_{t_2}=\alpha(\theta_{est}\mid_{t_1} + G\Delta t) + \beta \theta_{A_x} & \quad\text{… (3)} \\
\alpha+\beta=1 & \quad\text{… (4)} \\
\end{array} \right.
\]

And as we discussed previously, the above method is essentially a special case of the more generic Kalman filter. And here is the implementation of the equations above:

float EstimateAngle() {
    currTimeStamp = micros();
    elapsedTime = currTimeStamp - prevTimeStamp;
    prevTimeStamp = currTimeStamp;

    elapsedTimeSec = (float) elapsedTime / 1e6f;

    float angle = GetAccAngle();
    if (isFirstTime) {
        currAngle = angle;
        isFirstTime = false;
    } else {
        VxGyro = analogRead(PIN_GYRO_X);
        degX = (float) (VxGyro - GYRO_X_INIT_VAL) * 3300.0f / 512.0f / 180.0f * (float) elapsedTimeSec;
        currAngle = 0.95 * (prevAngle + degX) + 0.05 * angle;
    }

    prevAngle = currAngle;

    //Debugging code omitted here, see attached source for the full code listings.

    return currAngle;
}

The elapsed time is calculated by measuring the time spent between two consecutive function calls to EstimateAngle(). Alternatively, you can use hardware timer interrupt in your code so that the time interval is precisely known beforehand. Because the readings from the gyroscope come from the ADC directly, they will need to be converted to the actual angles being measured.

In the code above, degX records the rotated angle within the measurement time interval. Since the ADC resolution is 10 bit on ATmega328, the supply voltage is 3.3V (3300 mV) and the 4X gyroscope output sensitivity is 2mV/degree, the degree measured in radians can be calculated as:

\[\theta=\frac{V_{gyro}\times 3300\times 2}{2^{10}\times 180}\Delta t=\frac{V_{gyro}\times 3300}{512\times 180}\Delta t\]

We could’ve just used the above formula as the final estimate for the inclination angle as the above calculation matches the filtering algorithm we discussed earlier.

In my particular implementation, I added another low-pass filter for the combined sensor output in order to achieve better stability. The reason for adding this low-pass filter is as the following:

  • The control loop is relatively fast (e.g. 10ms per iteration). Since the time constants for the motors tend to be much longer, a 50ms control loop time interval is more than adequate. In this case, we could just add some delay or use a hardware timer to make the interval longer. But since the angular acceleration is not a constant during the measurement interval, we will loose some accuracy in the inclination angle calculation if the time interval is too long.
  • The wheels of the toy truck I used are roughly 3 inches in diameter, which are pretty small compared to the size of the robot. The small wheel size makes it more sensitive to the surface conditions, which translates into larger variations in sensor readings. By averaging over several readings, we can decrease the sensor variations.

That said, this last step may not be necessary in your case and you can simply set the ANGLE_BUFFER_LENGTH to 1 to bypass the buffer. The code for calculating the average is shown below. The variable angleBuffer is a circular buffer and the buffer values are updated within the main loop.

float GetAvgAngle() {
    int count = 0;
    float a = 0;

    for (int i = 0; i < ANGLE_BUFFER_LENGTH; i++) {
        if (angleBuffer[i] != 0) {
            count++;
            a += angleBuffer[i];
        }
    }

    if (count > 0)
        return a / float(count);
    else
        return 0.0;
}

The PID control loop

Motors can be controlled via the classic PID control. Microchip’s application notes AN964 has some excellent explanation on the physics and math behind the PID control, and the notions in my implementation mirrors those in the application notes.

Here is the code for the main loop:

 
void loop() {
    float a = EstimateAngle() - INIT_ANGLE;

    angleBuffer[angleBufferIndex] = a;
    angleBufferIndex = (angleBufferIndex + 1) % ANGLE_BUFFER_LENGTH;
    float ang = GetAvgAngle();

    curErr = ang - INIT_ANGLE; //error    
    SumErr += curErr;
    
    if (SumErr > SumErrMax) SumErr = SumErrMax;
    else if (SumErr < SumErrMin) SumErr = SumErrMin;
    
    //Ki*SumE/(Kp*Fs*X) 
    integralTerm = SumErr * elapsedTimeSec * Ki / Kp * 10.0; 
    derivativeTerm = curErr - prevErr;
    
    if(derivativeTerm > 0.1) derivativeTerm = 0.1;
    else if (derivativeTerm < -0.1) derivativeTerm = -0.1;
    
    // Kd(curErr-prevErr)*Ts/(Kp*X)
    derivativeTerm = derivativeTerm * Kd * elapsedTimeSec / Kp; 
    
    if(derivativeTerm > 120) derivativeTerm = 120;
    else if (derivativeTerm < -120) derivativeTerm = -120;
    
    Cn = (curErr + integralTerm + derivativeTerm) * Kp / 10.0;
    Serial.println(Cn);
    WheelDirection dir;
    
    if (Cn > 0) dir = DIR_FORWARD;
    else if (Cn < -0) dir = DIR_BACKWARD;
    else dir = DIR_STOP;
       
    throttle = abs(Cn);
        
    if (abs(ang) > 0.7)  MoveWheels(DIR_STOP, 0); //if angle too large to correct, stop motor        
    else MoveWheels(dir, throttle);
    
    prevErr = curErr;
}

In plain English, the control loop works like this:

  • The current inclination angle is estimated and stored in the circular buffer. The averaged inclination angle measurement is fed into the PID controller. As mentioned earlier, if no averaging is needed, the current inclination estimate can be used directly by setting the buffer length to 1.
  • Parameters of the PID controller is calculated using the current error measurement (curErr) and the cumulative error (SumErr).
  • We use the output of the PID controller (Cn) as the “throttle” to control the motors. The sign of the Cn corresponds to the motor rotation direction.
  • If the inclination angle is too large (e.g. greater than roughly 40 degree), there is no way for the robot to correct its position anyway so we will simply stop the motor. This condition is needed to prevent the wheels from moving when the robot is positioned horizontally.
  • The current error term is assigned to prevErr and now we are ready for the next loop.

Tuning and trouble-shooting

Now it’s time to power on the robot. Don’t expect it will work the very first time even if you did the programming correctly since there are many parameters involved in the whole process. Here are a few simple steps that you could use to ensure that the robot is functioning correctly and making parameter adjustments so that it could balance.

  1. Set the mode switch so that the motor is turned off and the debug information can be viewed via a serial console.
  2. Power up the robot and lie it flat, the angle reported from the accelerometer should be close to ±1 depending on which side is facing up (if you are using sine instead of approximating it the value read should be close to ±Pi/2) and the gyroscope reading should be around 0 since the robot is stationary.
  3. Hold the robot up straight to its balanced position, the accelerometer angle reading should be close to 0 and the gyroscope reading should remain around 0 when there is no movement. Pay attention to the angle reading when the robot is balanced, it may be slightly off zero due to the actual weight distribution of the robot. And if this value is none-zero, you will need to set the INIT_ANGLE to the observed value when the robot is in-balance so that this offset value can be subtracted from the actual measurements.
  4. Now try move the robot back and forth about its balanced position, you should see both the accelerometer and the gyroscope readings oscillate around 0. Pay special attention to the sign of the gyroscope readings. It should match the sign of the accelerometer reading when the the robot is moving away from its balanced position.
  5. Now set the mode switch to normal operation mode. Upon powering up, hold the robot in the air and move it gently around its balanced position. You should see the wheels move towards the direction of the inclination. And the motor should spin faster when the inclination angle is larger.

If the above test steps are successful, we are already well on our way to a successful build. Now it’s time to tune the PID controller. The easiest way to tune a PID controller is to tune the P, I and D parameters one at a time. You can refer to Microchip’s application notes AN964 for more details. Basically, you will need to:

  1. Set I and D term to 0, and adjust P so that the robot starts to oscillate (move back and forth) about the balance position. P should be large enough for the robot to move but not too large otherwise the movement would not be smooth.
  2. With P set, increase I so that the robot accelerates faster when off balance. With P and I properly tuned, the robot should be able to self-balance for at least a few seconds.
  3. Finally, increase D so that the robot would move about its balanced position more gentle, and there shouldn’t be any significant overshoots.

There are also a couple of constants in the I and D term calculations, they will need to be adjusted by experiments to determine the optimal values.

If you are unable to balance the robot with the PID parameter adjustments mentioned above, chances are that the accelerometer or the gyroscope values are not calculated correctly and you will need to go back to the basic calibration steps mentioned earlier and see whether the readings from gyroscope and the accelerometer are handled properly. One common mistake is the gyroscope angle calculation, you will need to use the correct sensitivity according to the datasheet of the particular gyroscope you are using along with the formula we discussed earlier to come up with the correct angle.

The choice of the values of a and b in equation (4) affects the time constants of the control and can be fine tuned with the control loop interval and ANGLE_BUFFER_LENGTH parameter to achieve the best result. In my case the weight I used is 0.95 for the gyroscope readings and 0.05 for the accelerometer readings. The ANGLE_BUFFER_LENGTH setting and the value for b affect the time constant (or low pass filter cutoff frequency) in opposite manner. When you increase the buffer length, the time constant becomes higher (cutoff frequency is lower). But when b is incremented, the accelerometer’s time constant actually becomes lower (cutoff frequency is higher).

Here are a couple of pictures of the robot in its balanced positions:

Self-Balancing Robot in Action 1

Self-Balancing Robot in Action 1

Self-Balancing Robot in Action 2

Self-Balancing Robot in Action 2

And the following is a short video of the balancing robot in action. The stability of the robot can be dramatically improved if you use a properly designed gearbox that has negligible gear backlash. The backlash in a typical toy gearbox is usually quite significant. In my case, the wheels can be turned by a few degrees in either direction before the gears are engaged, and this makes it harder to control the robot accurately. Also as mentioned earlier, larger wheels can also improve stability as the robot will be less affected by the surface condition.


View on YouTube in a new window

Download

Self-balancing robot source code

Be Sociable, Share!

62 Comments

  1. rob4white says:

    Hi Kerry,

    Congrats on the balancing robot! I am at the stage of implementing the correct PID parameters myself and having some trouble to say the least…

    I have read up on this blog and the notes that you have referenced about the correct way to tune the controller but I am still failing to achieve balance and I am confident that the sensors were correctly calibrated with the correct values for their sensitivity etc.

    Is there any easier approach to achieving the correct values for the P I and D values than simply changing them slightly and reloading the code..?

    Many thanks!

    Rob

    • kwong says:

      Thanks Rob,

      What you could do is use a few potential meters on analog pins and use the voltage readings to adjust the P, I, D values. This way, you could do it on-the-fly without having to recompile and reload the code each time.

      Good luck!

  2. rob4white says:

    Thanks for the advice! Yeah I was thinking along them lines as well. I don’t if you are aware of the original thread started on the arduino forum for the balancing robot for dummies by Kas..?

    He posted some coding that allowed one to change the PID values via the serial monitor… I’m a little confused how this works but it might also be a useful tool for me… The relevant coding is:
    ——-Code——-

    int getTuning() {
    if(!Serial.available()) return 0;
    delay(10);
    char param = Serial.read(); // get parameter byte
    if(!Serial.available()) return 0;
    char cmd = Serial.read(); // get command byte
    Serial.flush();
    switch (param) {
    case ‘p’:
    if(cmd==’+’) Kp++;
    if(cmd==’-‘) Kp–;
    break;
    case ‘i’:
    if(cmd==’+’) Ki++;
    if(cmd==’-‘) Ki–;
    break;
    case ‘d’:
    if(cmd==’+’) Kd++;
    if(cmd==’-‘) Kd–;
    break;
    case ‘k’:
    if(cmd==’+’) K +=0.2;
    if(cmd==’-‘) K -=0.2;
    break;
    default:
    Serial.print(“?”); Serial.print(param);
    Serial.print(” ?”); Serial.println(cmd);
    }
    Serial.println();
    Serial.print(“K:”); Serial.print(K);
    Serial.print(” Kp:”); Serial.print(Kp);
    Serial.print(” Ki:”); Serial.print(Ki);
    Serial.print(” Kd:”); Serial.println(Kd);
    }

    Any thoughts or comments on this alternative approach?

    Cheers!

    • kwong says:

      Unless your timing is controlled via an interrupt, I wouldn’t use this method (via Serial console) to change the P/I/D settings.

      The reason is that the Arduino Serial library adds significant delays to the loop which may skew your tuning results. But it could work well if the effect of the added delay is accounted for.

      The code you included basically just reads from the serial console and if you press the “+” or “-” keys the corresponding value of Kp/Ki/Kd will change accordingly.

  3. rob4white says:

    Thanks for the advice Kerry, I just ran it there and I see what you mean about the delays with the loop timing… Seem to be having an unrelated power issue to the motors right now!! Hoping its nothing major or I could be set back!! :O

    I’ll let you know how I get on and thanks again, I fully appreciate how much is involved in creating a balancing bot! :)

  4. JD says:

    I’ve wanted to build a balancing robot for a while as a way to jump into learning electronics but I could never justify the cost of the pre-made sensors. I just read through all three parts you posted and ordered everything to build my own. I ended up getting the MMA8450QT instead. I was already placing a big order from a supplier and that’s all they had. It looks like it runs at a lower voltage so I think it should still work but will give less resolution in the angle measurement? Thanks for the great write up!

    • kwong says:

      Hi JD,

      Since MMA8450QT works below 1.8 V, you will need to use a comparable voltage reference attached to the Vref pin of the ATmega328 in order not to lose resolution.

      • Ugi says:

        The ATMega has an internal 2.56V reference voltage that could be used to give a pretty good range with a 1.8V sensor. In Arduino this would be analogReference(INTERNAL2V56). Be careful that you don’t have 5V connected to the Vref pin at the time or I think it might short your 5V to the 2V56 ref voltage.

        You would get 70% of the full resolution that way without needing extra parts.

        Ugi

  5. Jordan says:

    i may have to try to fix mine now :P

  6. starlino says:

    Excellent tutorial! For anyone interested I have several articles regarding sensor fusion on my blog http://www.starlino.com/ see under “IMU Theory and Experiments”.

  7. Ugi says:

    Great project – I must have a go some time.

    Ugi

  8. angelique says:

    the same project that our professor wants us to make. can you help me with that? please email me.. thank you very much guys for the help!

  9. Antonio Banderas says:

    Hi Kerry!

    When a try to run it I get the following errors:
    error: variable or field ‘MoveWheels’ declared void
    error: ‘WheelDirection’ was not declared in this scope
    error: expected primary-expression before ‘int’

    I just copied your code, so I don’t know…

    • kwong says:

      Hi Antonio,

      Which IDE are you using? The I2C library used is not the default one supplied with the Arduino IDE, did you download the I2C library?

      • Antonio Banderas says:

        I use Arduino 1.0.1
        i downloaded the I2C.h and put it to the right place, If I’d remove it, there would be a lot more error.

        • kwong says:

          Hi Antonio,

          I tried the code in Arduino 1.0.1 and indeed had the same issue as you reported. It appears that for whatever reason the environment does not recognize the enum WheelDirection defined earlier. This could be a defect of the Arduino 1.0.1 environment as in NetBeans and Eclipse, this code compiled with no problems.

          Anyway, a quick work around for this is to declare the dir as a global variable (adding to the end of the WheelDirection definition):

          enum WheelDirection {
          DIR_FORWARD,
          DIR_BACKWARD,
          DIR_STOP
          } dir;

          and remove the first parameter from function MoveWheels altogether:

          void MoveWheels(int speed) {
          }

          I have created a modified version of the code that does just this and I had verified it compiles using Arduino 1.0.1.

          Let me know how it goes.

          • Antonio Banderas says:

            Hi Kerry.
            Thx for the code, now I can compile it, but doesn’t work. I have tested the robot with a simple motor driver (move forward, move back, etc..), so the building is correct.
            I think there is still something wrong with the code. Have you tested it on your robot?

          • kwong says:

            In my particular implementation there is a debug switch attached (see the following code snippet):

            Serial.print(“Motor is “);
            if (digitalRead(PIN_MODE_SWITCH) == HIGH) {
            MotorOff = true;
            Serial.println(“OFF”);
            }
            else {
            MotorOff = false;
            Serial.println(“ON”);
            }

            so if the switch is closed the motor will not move. This is used while tuning the code so you can see the output of the sensor values. I’d check that first and if it still does not work, if you could post some of the sensor value print outs in various positions then we can go from there…

          • Antonio Banderas says:

            hi Kerry,

            So, there is the log, I made it with the arduino module (and not with the robot itself)
            Also, I used a different kind of accccelerometer: MMA8451Q. It’s similar to yours…

            the date:

            Motor is OFF
            1A
            GyroX Stationary=463.32
            currAngleAvg=0.0000 currAngle=0.0333 accAngle=0.83 gyroAngle=-0.01 VxGyro=463 elapsedTime=758288 throttle=0.00
            0.00
            currAngleAvg=0.0383 currAngle=0.0724 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=81552 throttle=0.00
            0.01
            currAngleAvg=0.0579 currAngle=0.1116 accAngle=0.88 gyroAngle=-0.00 VxGyro=463 elapsedTime=122728 throttle=0.01
            0.01
            currAngleAvg=0.0774 currAngle=0.1466 accAngle=0.84 gyroAngle=-0.00 VxGyro=463 elapsedTime=123752 throttle=0.01
            0.01
            currAngleAvg=0.1152 currAngle=0.1795 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.01
            0.02
            currAngleAvg=0.1509 currAngle=0.2181 accAngle=0.89 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.02
            0.02
            currAngleAvg=0.1864 currAngle=0.2490 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.02
            0.02
            currAngleAvg=0.2206 currAngle=0.2806 accAngle=0.82 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.02
            0.03
            currAngleAvg=0.2543 currAngle=0.3115 accAngle=0.84 gyroAngle=0.00 VxGyro=464 elapsedTime=122720 throttle=0.03
            0.03
            currAngleAvg=0.2854 currAngle=0.3370 accAngle=0.85 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.03
            0.03
            currAngleAvg=0.3147 currAngle=0.3591 accAngle=0.81 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.03
            0.03
            currAngleAvg=0.3408 currAngle=0.3864 accAngle=0.85 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.03
            0.04
            currAngleAvg=0.3658 currAngle=0.4086 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.04
            0.04
            currAngleAvg=0.3897 currAngle=0.4335 accAngle=0.85 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.04
            0.04
            currAngleAvg=0.4145 currAngle=0.4533 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.04
            0.04
            currAngleAvg=0.4368 currAngle=0.4709 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.04
            0.05
            currAngleAvg=0.4576 currAngle=0.4926 accAngle=0.85 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.05
            0.05
            currAngleAvg=0.4773 currAngle=0.5091 accAngle=0.85 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.05
            0.05
            currAngleAvg=0.4959 currAngle=0.5251 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.05
            0.05
            currAngleAvg=0.5139 currAngle=0.5453 accAngle=0.87 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.05
            0.05
            currAngleAvg=0.5315 currAngle=0.5591 accAngle=0.85 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.05
            0.06
            currAngleAvg=0.5482 currAngle=0.5714 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.5636 currAngle=0.5844 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.5766 currAngle=0.5941 accAngle=0.81 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.5883 currAngle=0.6043 accAngle=0.82 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.5993 currAngle=0.6148 accAngle=0.84 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.6094 currAngle=0.6263 accAngle=0.87 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.6201 currAngle=0.6326 accAngle=0.78 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.6296 currAngle=0.6432 accAngle=0.87 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.06
            currAngleAvg=0.6391 currAngle=0.6518 accAngle=0.84 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.06
            0.07
            currAngleAvg=0.6476 currAngle=0.6591 accAngle=0.82 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.07
            0.07
            currAngleAvg=0.6564 currAngle=0.6732 accAngle=0.88 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.07
            0.07
            currAngleAvg=0.6663 currAngle=0.6829 accAngle=0.89 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.07
            0.07
            currAngleAvg=0.6767 currAngle=0.6916 accAngle=0.88 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.07
            0.07
            currAngleAvg=0.6875 currAngle=0.7011 accAngle=0.82 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.07
            0.07
            currAngleAvg=0.6968 currAngle=0.7076 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.07
            0.07
            currAngleAvg=0.7051 currAngle=0.7166 accAngle=0.83 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.07
            0.07
            currAngleAvg=0.7134 currAngle=0.7226 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.07
            0.07
            currAngleAvg=0.7206 currAngle=0.7322 accAngle=0.86 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.07
            0.07
            currAngleAvg=0.7288 currAngle=0.7378 accAngle=0.87 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.07
            0.07
            currAngleAvg=0.7359 currAngle=0.7450 accAngle=0.82 gyroAngle=0.00 VxGyro=464 elapsedTime=123-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8080 currAngle=0.8028 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8081 currAngle=0.8042 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8082 currAngle=0.8050 accAngle=0.85 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8090 currAngle=0.8025 accAngle=0.78 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8089 currAngle=0.8034 accAngle=0.85 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8086 currAngle=0.8035 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8081 currAngle=0.8023 accAngle=0.81 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8081 currAngle=0.8024 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8077 currAngle=0.8038 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8078 currAngle=0.8026 accAngle=0.81 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8080 currAngle=0.8040 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8085 currAngle=0.8113 accAngle=0.98 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8110 currAngle=0.8189 accAngle=0.99 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8164 currAngle=0.8078 accAngle=0.71 gyroAngle=-0.01 VxGyro=462 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8177 currAngle=0.5368 accAngle=0.97 gyroAngle=-0.29 VxGyro=397 elapsedTime=123760 throttle=0.08
            0.07
            currAngleAvg=0.7262 currAngle=0.3289 accAngle=0.11 gyroAngle=-0.20 VxGyro=419 elapsedTime=123760 throttle=0.07
            0.06
            currAngleAvg=0.5629 currAngle=0.3857 accAngle=-0.02 gyroAngle=0.08 VxGyro=481 elapsedT44 currAngle=-0.3394 accAngle=-0.41 gyroAngle=0.25 VxGyro=518 elapsedTime=127920 throttle=0.04
            -0.05
            currAngleAvg=-0.4608 currAngle=0.0623 accAngle=0.39 gyroAngle=0.38 VxGyro=548 elapsedTime=126880 throttle=0.05
            -0.03
            currAngleAvg=-0.2827 currAngle=0.2632 accAngle=0.29 gyroAngle=0.20 VxGyro=508 elapsedTime=124800 throttle=0.03
            0.00
            currAngleAvg=0.0004 currAngle=0.0346 accAngle=-0.16 gyroAngle=-0.22 VxGyro=414 elapsedTime=123752 throttle=0.00
            0.01
            currAngleAvg=0.1251 currAngle=-0.2462 accAngle=-0.20 gyroAngle=-0.28 VxGyro=400 elapsedTime=124800 throttle=0.01
            0.00
            currAngleAvg=0.0222 currAngle=-0.4905 accAngle=0.12 gyroAngle=-0.28 VxGyro=402 elapsedTime=125840 throttle=0.00
            -0.02
            currAngleAvg=-0.2290 currAngle=-0.4408 accAngle=-0.24 gyroAngle=0.04 VxGyro=472 elapsedTime=125848 throttle=0.02
            -0.04
            currAngleAvg=-0.3875 currAngle=-0.1498 accAngle=0.06 gyroAngle=0.28 VxGyro=525 elapsedTime=126880 throttle=0.04
            -0.04
            currAngleAvg=-0.3554 currAngle=0.2147 accAngle=0.58 gyroAngle=0.35 VxGyro=540 elapsedTime=125840 throttle=0.04
            -0.01
            currAngleAvg=-0.1203 currAngle=0.3082 accAngle=0.16 gyroAngle=0.10 VxGyro=486 elapsedTime=124800 throttle=0.01
            0.01
            currAngleAvg=0.1294 currAngle=0.1511 accAngle=0.06 gyroAngle=-0.15 VxGyro=429 elapsedTime=123760 throttle=0.01
            0.02
            currAngleAvg=0.2297 currAngle=-0.2043 accAngle=0.23 gyroAngle=-0.38 VxGyro=378 elapsedTime=123752 throttle=0.02
            0.01
            currAngleAvg=0.0900 currAngle=-0.3811 accAngle=-0.40 gyroAngle=-0.18 VxGyro=424 elapsedTime=124800 throttle=0.01
            -0.01
            currAngleAvg=-0.1398 currAngle=-0.2705 accAngle=-0.30 gyroAngle=0.11 VxGyro=488 elapsedTime=126888 throttle=0.01
            -0.03
            currAngleAvg=-0.2803 currAngle=0.1309 accAngle=0.71 gyroAngle=0.37 VxGyro=545 elapsedTime=126880 throttle=0.03
            -0.02
            currAngleAvg=-0.1686 currAngle=0.3050 accAngle=0.16 gyroAngle=0.18 VxGyro=504 elapsedTime=124800 throttle=0.02
            0.01
            currAngleAvg=0.0601 currAngle=0.2636 accAngle=0.26 gyroAngle=-0.04 VxGyro=454 elapsedTime=123752 throttle=0.01
            0.02
            currAngleAvg=0.2381
            currAngleAvg=0.7827 currAngle=0.7837 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.08
            0.08
            currAngleAvg=0.7856 currAngle=0.7838 accAngle=0.81 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7884 currAngle=0.7854 accAngle=0.84 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7893 currAngle=0.7859 accAngle=0.82 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7900 currAngle=0.7868 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7910 currAngle=0.7885 accAngle=0.85 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7921 currAngle=0.7884 accAngle=0.81 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7929 currAngle=0.7905 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7942 currAngle=0.7968 accAngle=0.86 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.7969 currAngle=0.7963 accAngle=0.81 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.08
            0.08
            currAngleAvg=0.7995 currAngle=0.7980 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8020 currAngle=0.7996 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8030 currAngle=0.7999 accAngle=0.83 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8042 currAngle=0.8014 accAngle=0.86 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8053 currAngle=0.8011 accAngle=0.82 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8058 currAngle=0.8068 accAngle=0.86 gyroAngle=0.00 VxGyro=464 elapsedTime=123760 throttle=0.08
            0.08
            currAngleAvg=0.8081 currAngle=0.8075 accAngle=0.85 gyroAngle=-0.00 VxGyro=463 elapsedTime=122720 throttle=0.08
            0.08
            currAngleAvg=0.8101 curr86 elapsedTime=123760 throttle=0.04
            0.04
            currAngleAvg=0.4137 currAngle=0.0080 accAngle=-0.27 gyroAngle=-0.16 VxGyro=427 elapsedTime=124800 throttle=0.04
            0.03
            currAngleAvg=0.2658 currAngle=0.4573 accAngle=-1.00 gyroAngle=0.53 VxGyro=581 elapsedTime=124800 throttle=0.03
            0.02
            currAngleAvg=0.2218 currAngle=0.3346 accAngle=-0.96 gyroAngle=-0.05 VxGyro=451 elapsedTime=123760 throttle=0.02
            0.03
            currAngleAvg=0.2716 currAngle=0.0072 accAngle=-0.33 gyroAngle=-0.31 VxGyro=394 elapsedTime=124800 throttle=0.03
            0.03
            currAngleAvg=0.2713 currAngle=-0.1130 accAngle=0.09 gyroAngle=-0.13 VxGyro=434 elapsedTime=124800 throttle=0.03
            0.01
            currAngleAvg=0.0813 currAngle=0.2801 accAngle=-0.97 gyroAngle=0.46 VxGyro=566 elapsedTime=124800 throttle=0.01
            0.01
            currAngleAvg=0.0631 currAngle=0.2206 accAngle=-0.88 gyroAngle=-0.00 VxGyro=463 elapsedTime=123760 throttle=0.01
            0.01
            currAngleAvg=0.1342 currAngle=-0.1179 accAngle=-0.75 gyroAngle=-0.31 VxGyro=395 elapsedTime=124800 throttle=0.01
            0.01
            currAngleAvg=0.1326 currAngle=-0.0689 accAngle=-0.22 gyroAngle=0.06 VxGyro=476 elapsedTime=125840 throttle=0.01
            0.00
            currAngleAvg=0.0163 currAngle=0.1932 accAngle=-1.00 gyroAngle=0.32 VxGyro=536 elapsedTime=124800 throttle=0.00
            0.00
            currAngleAvg=0.0071 currAngle=-0.0118 accAngle=-0.93 gyroAngle=-0.16 VxGyro=428 elapsedTime=123760 throttle=0.00
            0.00
            currAngleAvg=0.0425 currAngle=-0.3092 accAngle=-0.45 gyroAngle=-0.29 VxGyro=399 elapsedTime=125840 throttle=0.00
            -0.00
            currAngleAvg=-0.0376 currAngle=-0.1247 accAngle=-0.56 gyroAngle=0.21 VxGyro=509 elapsedTime=126888 throttle=0.00
            -0.01
            currAngleAvg=-0.1436 currAngle=0.0292 accAngle=-0.99 gyroAngle=0.21 VxGyro=509 elapsedTime=126880 throttle=0.01
            -0.01
            currAngleAvg=-0.1299 currAngle=0.0296 accAngle=-0.71 gyroAngle=0.04 VxGyro=472 elapsedTime=125840 throttle=0.01
            -0.00
            currAngleAvg=-0.0170 currAngle=0.0373 accAngle=0.04 gyroAngle=0.01 VxGyro=465 elapsedTime=125840 throttle=0.00
            0.00
            currAngleAvg=0.0370 currAngle=0.0099 accAngle=-0.32 gyroAngle=-0.01 VxGyro=461 e

            thx

          • kwong says:

            Hi Antonio,

            By looking at the traces, it appeared that the angle measurements were working (both accelerometer and gyroscope). What appeared to be the problem is that the “throttle” value is way too low. If you take a look at the code:

            Cn = (curErr + integralTerm + derivativeTerm) * Kp / 10.0;

            the throttle has a scaling factor there, which is typically applied experimentally. I would recommend tuning the scaling factor in the calculations for Cn/derivative term/integral term and see if you get the desired value ranges. Your goal is to set this value so that it’s absolute maximum value (e.g. when tilting the robot forward or backward) is around the maximum PWM resolution (255).

          • Antonio Banderas says:

            So, I changed the terms, now the throttle is around 150-200, but the motor still doesn’t work. When I turn on the robot, the (gyro calibration) LED is flashes for a second and then nothing happens. I move the sensors in every possible way, but really nothing. :(
            And as I said, I tested the motor with a simple code, so the driver and everything seems good.

          • kwong says:

            Hmm… this is a bit puzzling as the throttle output is directly used to drive the motor via PWM output. So if the drive circuit is working properly (which I am sure it is as you indicated), you should see the wheels move.

            Do you have an oscilloscope at hand? If so could you see if the PWM pins have any activities when the robot is tilted? If not the only suggestion I have is to try a different chip since the PWM output should work.

  10. keith says:

    Hi Kerry, great article. I am building one also and am trying to put some code together. I am using
    the MPU6050 chip and have it all setup and have some action happening with the wheels. (Not quite
    ready to hit the ground yet though.)

    I was not able to download your code as I am using Windows, but have grabbed the key parts from the
    snippets posted above.
    I think since I am using a different sensor, the measurements are in different scales.

    Would you be able to share your readings for when the robot is upright?

    Mine shows for standing: X_acc: -309 Y_acc: -747 Z_acc: 15700 X_gyro: -101 Y_gyro: -66
    Z_gyro: 75
    When wheels in the air: X_acc: 1092 Y_acc: 114 Z_acc: -17007 X_gyro: -108 Y_gyro: -68
    Z_gyro: 73

    What is the range for the currAngle? Is it between 0 and 1? and also the degX range?

    • kwong says:

      Hi Keith,

      The accelerometer readings are dependent on the resolution. it should be within [-2^(n-1), 2^(n-1)), where n is the bit of resolution. So for a 10 bit accelerometer, the x reading is between -512 and 511. And the angles in the code is always between [-pi/2, pi/2]. In my code the angle is limited to [-0.7, 0.7] (roughly 40 degrees) so that when the robot does fall the motor can be stopped to prevent damage.

      The gyroscope reading is specified in the datasheet and usually is degree/second. So if the sampling period is 10ms and the reading is 5 for example, it should be interpreted as 500 degree per second or roughly 2.78.

    • lezav says:

      Hi, I also built a 2 wheel bot using the 6050. Have you been able to get the 6050 to work with this code? Thanks

      • keith says:

        hi, had a long break on this project. Tried several code examples and haven’t hit the jackpot yet. Actually just got it out last week for another attempt. Got myself some potentiometers for adjusting the pids. Also got a serial bluetooth so I hopefully can program it from my tablet using arduino comander.

    • Stuart says:

      Hi Keith

      I am just trying to do the same thing with the MPU6050. I have some errors returned within the Arduino 1.0.1. If you have this sorted out could you share the code you have?

      Hate to do something twice, when it may have already been done elsewhere.

      Thanks

  11. Phuong says:

    Hi! Im Phuong. I want to make 2 wheel balacing robot use stm32f4discovery. Please, you can help me
    Thanks

  12. dean says:

    Hi Kerry,

    I downloaded your code (main.ino) following your discussion with Antonio above because I’m using Arduino 1.0.1.

    Tried to compile the sketch and got this error:

    c:/users/canafrancad/desktop/arduino-1.0.1/hardware/tools/avr/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o:(.init9+0x0): undefined reference to `main’

    I checked if the file exists and it does. I’m a bit new to arduino and a little bit lost in this, can you help?

    • kwong says:

      Hi Dean,

      Try start a new sketch, copy and paste the code into the new sketch from main.ino and compile, it should compile without any errors.

      There is some issue with how Arduino IDE interprets the file name and unfortunately, main.ino confused it. If you rename the file to anything other than main.ino it should work. (I was not compiling it using Arduino’s IDE and thus didn’t encounter this behavior).

      • dean says:

        Hi Kerry,

        Thank you very much, you’re correct, renaming it to kerry.ino solves the problem and it compiles without errors.

        Since I know now that it compiles correctly and since I only have an IMU with BMA180 and ITG3200 with I2C interface I will try to modify your code to fit this. I can read values from these devices and my only concern would be the difference in specs between your sensors and mine.

        regards,

        Dean

  13. sanjay says:

    Hey Kerry,

    I was looking at your code and I had 2 specific questions about 2 lines:

    1) // Kd(curErr-prevErr)*Ts/(Kp*X)
    derivativeTerm = derivativeTerm * Kd * elapsedTimeSec / Kp;

    Shouldn’t elapsedTimeSec (Ts) be in the denominator?

    2) Cn = (curErr + integralTerm + derivativeTerm) * Kp / 10.0;
    Serial.println(Cn);

    Did you intend to remove the Serial.println (Cn) line? I find that the print statements add appreciable delay.

    Thanks for a great article.

    Sanjay

    • kwong says:

      Hi Sanjay,

      If you take a look at the calculations in AN964 (Eq. 6 through 9), you can derive the equation used here. I just took a look, Ts should be on the numerator as shown.

      Regarding your second question, yes I wrote out Cn during testing and that line should have been commented out.

  14. Jason says:

    Hi Kerry,
    Can you briefly explain the meaning of this two statements?

    if(derivativeTerm > 0.1) derivativeTerm = 0.1;
    else if (derivativeTerm 120) derivativeTerm = 120;
    else if (derivativeTerm < -120) derivativeTerm = -120;

    Thanks!

  15. Jason says:

    :/ this is weired…
    I will just post the two separately than

    if(derivativeTerm > 0.1) derivativeTerm = 0.1;
    else if (derivativeTerm < -0.1) derivativeTerm = -0.1;

  16. Jason says:

    Also, when I tilt the robot, both “accAngle” and “gyroAngle” should either increase or decrease together but not “one goes up and the other goes down”, is that that correct?
    (I think the direction print on my board is incorrect somehow)

    Thanks,

  17. Charlie says:

    Comments and observations…

    There is no need to calculate the inclination angle. The inclination angular velocity reading from the gyro can go directly into a digital filter. The digital filter output goes directly to the motors. Simple and neat. The angle never needs to be calculated.

    There is no need for an accelerometer. I believe the accelerometer you are using in your design simply gives you positive feedback. Positive feedback is usually best avoided but in this case it stabilizes the loop. Some designs use encoder readings from the wheels to do the same thing. A much simpler solution is simply to produce the positive feedback within the digital filter. Simple and neat.

    There is no need for a Kalman filter. The robot cannot respond to the high frequency noise and the loop cancels the low frequency noise.

    Theory is on my website. You don’t need elaborate control theory. You just need math software to solve the system of differential equations.

  18. Wang says:

    Hey Kerry,
    I ran into a problem when trying to adjust my PID. Both acceleration and gyro angle are working fine, include INIT_ANGLE.
    However for some reason, my Cn output is always negative. When I tilt the sensors, Cn will go up or down according to the direction of tilting, but always stay in negative region.
    For my understanding of the code, shouldn’t the Cn being “centered” at zero, and goes positive for one direction and negative for the other?
    Please help me point out where I might need to further adjust.
    Thanks for the amazing documentary, it really helped me getting into using IMU.

    Wang,

  19. antonio says:

    Hello Kerry
    I tried to install netbeans but I had no success, then tried to load the program in the robot arduino 1:01 colcoquei and modified the code but not got success, you tested this code in your robot? Any help thank

  20. Usman says:

    Hello Kerry
    I am trying to build a self balancing robot for my final year project. I have just started. I need some help in selection of motors. I would really appreciate if you could help me with the specification of motors and how to decide those specifications.

  21. lezav says:

    I am in the 2nd step of buidling a 2 wheeled robot using an IMU6050. See below link for details… any help and comments are appreciated.
    http://letsmakerobots.com/node/38692#comment-108517

  22. Ayush says:

    I am using MPU 6050 IMU, it has a DMP which give precise angles. It costs 2$. However my bot is not balancing properly. I am using Arduino PID library

  23. Ayush says:

    Kerry,
    I was looking at the your code, did the motors respond at low values of pwm? My bot’s motors require atleast offset value of 35 before it starts.

  24. Guessist says:

    thanks a lot.. your awesome code make me happy..

  25. Alex says:

    hi kerry! Thanks for taking a time for sharing your balancing code!

    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

    • Alex says:

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

    • kwong says:

      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.

      • Alex says:

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

  26. Guy says:

    hello

    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.

  27. hakan says:

    why did you write INIT_ANGLE as -0.005 ? and how can i find it ?

  28. YT says:

    I’m building one as a school project, though i have a problem with tuning the PID. When you say “oscillating”, what do you actually mean? Should the robot be oscillating while balancing on its own? Or while i’m holding it? I can’t really wrap my head around that. I’ve read a lot of blogs, articles, sites, they all say “tune kP til you get oscillation, then add kI to make it react faster to change”, but i don’t understand how it is supposed to oscillate, as in, should it be self balanced oscillation? Or me holding it?

    • kwong says:

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

      • YT says:

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

  29. Kindly briefly describe me the working of PID controller in this project. Urgent

  30. Tig says:

    In the getAccXYZ() function you are reducing the accelerometer values by 1024 if there are more than 511
    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;
    }

  31. Atom says:

    Kerry,

    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]

Leave a Reply to Tig