Shop OBEX P1 Docs P2 Docs Learn Events
Balancing Car Project Attempt number 99 — Parallax Forums

Balancing Car Project Attempt number 99

Over the number of years now I have been dabbling with make a balancing robot work with the P1. I seem to always get hung up on the timing with the Gyro Scope and the Accelerometer.

Anyway the other day I came across this chassis that look nice and was fairly inexpensive.
Balance Chassis

Now how do I drive this thing. Parallax sells a motor driver MC33926 for $90 dollars. What that's a little steep for a motor controller. I check from the manufacturer's website and it sells for $65 there. Still a little pricey. Then I found this board:
This board is only $7. At that price I took two.
Now to drive this board is a snap. You only need 5 wires, Ground, M1A, M1B, M2A, M2B. That's it.
I went to work a coded up a driver for it in just a couple of minutes.

#include <stdio.h>
#include <stdlib.h>
#include "propeller.h"
#include "smartpins.h"
#include "mdd3a.h"

int _M1A, _M1B, _M2A, _M2B;

void mdd3a_Init(int m1a, int m1b, int m2a, int m2b)
    int f;

    f = 500 | 500 << 16;

    _M1A = m1a;
    _M1B = m1b;
    _M2A = m2a;
    _M2B = m2b;

    _pinstart(_M1A, P_OE | P_PWM_TRIANGLE, f, 0);
    _pinstart(_M1B, P_OE | P_PWM_TRIANGLE, f, 0);
    _pinstart(_M2A, P_OE | P_PWM_TRIANGLE, f, 0);
    _pinstart(_M2B, P_OE | P_PWM_TRIANGLE, f, 0);


void mdd3a_SetPWM(int pwm1, int pwm2)
    int set1, set2;

    if (pwm1 < 1000)
        pwm1 = 1000;
    if (pwm1 > 2000)
        pwm1 = 2000;
    if (pwm2 < 1000)
        pwm2 = 1000;
    if (pwm2 > 2000)
        pwm2 = 2000;

    set1 = pwm1 - 1500;
    set2 = pwm2 - 1500;

    if (abs(set1) < 5)
        set1 = 0;
    if (abs(set2) < 5)
        set2 = 0;

    if (set1 < 0)
        _wypin(_M1A, 0);
        _wypin(_M1B, -set1);
        _wypin(_M1B, 0);
        _wypin(_M1A, set1);

    if (set2 < 0)
        _wypin(_M2A, 0);
        _wypin(_M2B, -set2);
        _wypin(_M2B, 0);
        _wypin(_M2A, set2);


void mdd3a_Stop(void)

Smart pins made the job easier. The hard part was figuring out what all those codes mean to configure the smart pin and while I was at it, I made the driver compatible with an RC receiver.

Now for the hard part. I decided to use an ICM20948 chip. This is the re-engineered MPU9250 from Invensense which is now a part of TDK. Same frustrating chip with a new twist.

Anyway, I like a challenge and this chip aims to deliver. After working on it for about a week now I think I have the chip working enough to use in my project. The driver is massive with all the crazy stuff in the chip. It has a DMP processor on it but to use it require a lot more memory and time that I don't need to look at. The documentation is poor, and the code is only available in binary format.

For this project I only need the Gyro scope and the Accelerometer to work.

Here is a picture of the complete robot ready to fall over.
Balance Bot

The most expensive piece is the P2. Now I need to build some loop code that can make this beast stand up on its own.



  • Please post links for the chassis and boards?

  • I got them through digikey but they are made by Seeed Technology.

    Two Wheel Car

    Maker MDD3A Motor Controller

    Seeed Studio Website

    All of those items and more are out of stock on the Seeed website.


  • ercoerco Posts: 20,242

    Nice! Good luck on this, looking forward to seeing her balance.

  • Got the code put together and the robot looks like it's had too many to drink.

    This is where I'm lost. Is it too much P or not enough I or is it just D.

    The code has been tuned to the loop time which is about 1.3ms to read the gyro and accelerometer and calculate the speed percentage which is converted to an RC PWM value.

    I got the initial code off the internet and then modified it for my use. It's using a complementary filter and not Kalman. Don't want to make this complicated just yet.

    Here is the code that I'm using:

    #include <stdio.h>
    #include <stdlib.h>
    #include <math.h>
    #include <propeller.h>
    #include "icm20948.h"
    #include "mdd3a.h"
    #define ICMCLK 36
    #define ICMDTA 37
    #define DMM1A 20
    #define DMM1B 21
    #define DMM2A 22
    #define DMM2B 23
    float compFilter(float gyroX, float accY, float accZ);
    int i;
    //Complementary Filter vars, filter weight K
    const float K = 0.998F;
    float pitch = 0.0F;
    //Loop Time Value 
    const float DT = 0.001395F; // 0.001250
    const float KP = 2.5; //2.5F
    const float KI = (30.0F*0.001395f); 
    const float KD = (0.05F/0.001395f);
    float setPoint = -0.3F;
    int main(int argc, char** argv)
        int ms;
        short ax, ay, az;
        short gx, gy, gz;
        float currentError, previousError;
        float integral = 0.0F;
        float derivative = 0.0F;
        int speedPercent = 0;
        int pwm;
        i = ICM20948_Init(ICMCLK, ICMDTA);
        if (i != 0)
            printf("ICM20948 Not Found!\n");
            while (1)
        ICM20948_SetGyroOffsets(20, -13, 8);
        ICM20948_SetAccelOffsets(2450, 3496, -660);
        /* 4G */
        ICM20948_ConfigAccel(0, 1, 1);
        /* 2000DPS */
        ICM20948_ConfigGyro(0, 3, 1);
        mdd3a_Init(DMM1A, DMM1B, DMM2A, DMM2B);
        i = 1;
        while (1)
            ms = _getus();
            while ((gz = ICM20948_DataReady()) == 0)
            if (gz != 3)
            ICM20948_Gyro(&gx, &gy, &gz);
            ICM20948_Accel(&ax, &ay, &az);
            pitch = compFilter(gx/16, ay, az);
            currentError = (setPoint - pitch);
            integral = (integral + currentError);
            derivative = (currentError - previousError);
            speedPercent = ((KP * currentError) + (KI * integral) + (KD * derivative));
            if (speedPercent > 50)
                speedPercent = 50;
            if (speedPercent < -50)
                speedPercent = -50;
            pwm = (speedPercent * 5) + 1500;
            mdd3a_SetPWM(pwm, pwm);
            previousError = currentError;
            ms = _getus() - ms;
            if (abs(pitch) > 20)
                printf("Pulse: %d\n", pwm);
                while (1)
    float compFilter(float gyroX, float accY, float accZ)
        float acc = (180.0F / PI) * atan2(accY, accZ);
    //    printf("Gyro: %3.1f, Angle: %3.1f\n", gyroX, acc);
    //    _waitms(5000);
        return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * (acc)));

    Here is a video of the drunken robot:
    Balancing Bot

    You can see that it is bouncing back and forth.


  • Nice!

    When I experimented with such a balancing robot, the most astonishing finding was: If you enlarge the distance between the wheels and the balance point, making the robot higher, the stability will improve. Well, inertia will increase and base frequency will go down.
    Battery on top!

  • ercoerco Posts: 20,242

    Don't give up, sensor confusion ultimately leads to sensor fusion!

    I had modest goals and modest success with my simple BS2-powered balancer a decade ago.

  • At first guess, I would say your Proportional might be too low and or your Integral might be too high.

    Reference Page:

    Going down a similar rabbit hole a few years ago for a client, I designed a self leveling platform (Think Shipping Container).
    When first designing the code, it looked like the thing was at sea during a storm before I got the algorithm just right.

    Here is a scale model proof of concept I put together for the client before the funding approval.

  • Hanno Sander's "Balancing Robot" looked pretty interesting but I never did find a video of it in action:


Sign In or Register to comment.