Balancing Car Project Attempt number 99

iseries · 2023-02-23 23:08

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:
MDD3A
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);
    }
    else
    {
        _wypin(_M1B, 0);
        _wypin(_M1A, set1);
    }

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

}

void mdd3a_Stop(void)
{
    _pinf(_M1A);
    _pinf(_M1B);
    _pinf(_M2A);
    _pinf(_M2B);
}

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.

Mike

DaveJenson · 2023-02-24 15:41

Please post links for the chassis and boards?

iseries · 2023-02-24 16:52

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.

Mike

erco · 2023-02-24 18:22

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

iseries · 2023-02-26 13:54

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)
            _waitms(500);
    }

    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);

    _waitms(2000);

    printf("Starting...\n");

    i = 1;
    while (1)
    {
        ms = _getus();
        while ((gz = ICM20948_DataReady()) == 0)
            _waitus(1);

        if (gz != 3)
            _pinl(57);

        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)
        {
            mdd3a_Stop();
            _pinh(57);
            printf("Pulse: %d\n", pwm);

            while (1)
                _waitms(500);
        }
    }
}

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.

Mike

Christof Eb. · 2023-02-26 16:41

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!

erco · 2023-04-20 00:58

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.

Beau Schwabe · 2023-04-20 21:26

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

Reference Page:
https://www.clippard.com/cms/wiki/practical-pid-process-dynamics-proportional-pressure-controllers

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.

Mickster · 2023-04-21 16:35

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

notepad.org/articles/Programming_and_Customizing_the_Propeller.pdf

Craig

Balancing Car Project Attempt number 99

Comments