Shop Learn P1 Docs P2 Docs 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?

  • iseriesiseries Posts: 1,351

    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,222

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

  • iseriesiseries Posts: 1,351

    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!

Sign In or Register to comment.