BalaC Plus Balancing Robot

Periodically during the year I reach a "burnout" phase where everything gets to be a struggle.  With all that going on in the world and in my little corner of it, I've kinda hit a wall and was in need of some mindless diversion.

I've been wanting to do something with self-balancing robots and I recently got a BalaC Plus ESP32 Self-balancing Robot Kit from M5Stack.  It's a little pricey at $45 dollars for what is essentially a toy, but it reminded of stuff that dubbie does with all his robots.  Almost half of the price is for the M5StickC Plus which I use a lot.

It's a simple kit with 11 parts shown below.  It's using a couple of FM90 servo motors to drive the wheels, so it's relatively low performance.  It uses an STM32F030F4P6 with a couple of L9110S H-bridges to control the motor and the M5StickC Plus as the main controller.  The battery is a 16340, 3.7V, 700mAh.

Assembly is easy with only 11 parts and took under 10 minutes.

I downloaded the Arduino program from github and compiled and uploaded it to the ESP32.

BalaCplus.ino

//
// BalaC balancing robot (IMU:MPU6886)
// by Kiraku Labo
//
// 1. Lay the robot flat, and power on.
// 2. Wait until Gal-1 (Pitch Gyro calibration) completes.
// 3. Hold still the robot upright in balance until Cal-2 (Accel & Yaw Gyro cal)
// completes.
//
// short push of power button: Gyro calibration
// long push (>1sec) of power button: switch mode between standig and
// demo(circle)
//

#include <M5StickCPlus.h>

#define LED      10
#define N_CAL1   100
#define N_CAL2   100
#define LCDV_MID 60

boolean serialMonitor = true;
boolean standing      = false;
int16_t counter       = 0;
uint32_t time0 = 0, time1 = 0;
int16_t counterOverPwr = 0, maxOvp = 20;
float power, powerR, powerL, yawPower;
float varAng, varOmg, varSpd, varDst, varIang;
float gyroXoffset, gyroYoffset, gyroZoffset, accXoffset;
float gyroXdata, gyroYdata, gyroZdata, accXdata, accZdata;
float aveAccX = 0.0, aveAccZ = 0.0, aveAbsOmg = 0.0;
float cutoff            = 0.1;                     //~=2 * pi * f (Hz)
const float clk         = 0.01;                    // in sec,
const uint32_t interval = (uint32_t)(clk * 1000);  // in msec
float Kang, Komg, KIang, Kyaw, Kdst, Kspd;
int16_t maxPwr;
float yawAngle = 0.0;
float moveDestination, moveTarget;
float moveRate        = 0.0;
const float moveStep  = 0.2 * clk;
int16_t fbBalance     = 0;
int16_t motorDeadband = 0;
float mechFactR, mechFactL;
int8_t motorRDir = 0, motorLDir = 0;
bool spinContinuous = false;
float spinDest, spinTarget, spinFact = 1.0;
float spinStep  = 0.0;  // deg per 10msec
int16_t ipowerL = 0, ipowerR = 0;
int16_t motorLdir = 0, motorRdir = 0;  // 0:stop 1:+ -1:-
float vBatt, voltAve             = 3.7;
int16_t punchPwr, punchPwr2, punchDur, punchCountL = 0, punchCountR = 0;
byte demoMode = 0;


void imuInit();
void resetMotor();
void resetPara();
void resetVar();
void calib1();
void drvMotor(byte ch, int8_t sp);
void drvMotorL(int16_t pwm);
void drvMotorR(int16_t pwm);
void setMode(bool inc);
void dispBatVolt();
void getGyro();
void checkButtonP();
void calib2();
void startDemo();
void startDemo();
void drive();
void calDelay(int n);
void startDemo();
void sendStatus();
void readGyro();
void readGyro();


void setup() {
    pinMode(LED, OUTPUT);
    digitalWrite(LED, HIGH);
    M5.begin();
    Wire.begin(0, 26);  // SDA,SCL
    imuInit();
    M5.Axp.ScreenBreath(11);
    M5.Lcd.setRotation(2);
    M5.Lcd.setTextFont(4);
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setTextSize(1);
    resetMotor();
    resetPara();
    resetVar();
    calib1();
#ifdef DEBUG
    debugSetup();
#else
    setMode(false);
#endif
}

void loop() {
    checkButtonP();
#ifdef DEBUG
    if (debugLoop1()) return;
#endif
    getGyro();
    if (!standing) {
        dispBatVolt();
        aveAbsOmg = aveAbsOmg * 0.9 + abs(varOmg) * 0.1;
        aveAccZ   = aveAccZ * 0.9 + accZdata * 0.1;
        M5.Lcd.setCursor(30, 130);
        M5.Lcd.printf("%5.2f   ", -aveAccZ);
        if (abs(aveAccZ) > 0.9 && aveAbsOmg < 1.5) {
            calib2();
            if (demoMode == 1) startDemo();
            standing = true;
        }
    } else {
        if (abs(varAng) > 30.0 || counterOverPwr > maxOvp) {
            resetMotor();
            resetVar();
            standing = false;
            setMode(false);
        } else {
            drive();
        }
    }
    counter += 1;
    if (counter >= 100) {
        counter = 0;
        dispBatVolt();
        if (serialMonitor) sendStatus();
    }
    do time1 = millis();
    while (time1 - time0 < interval);
    time0 = time1;
}

void calib1() {
    calDelay(30);
    digitalWrite(LED, LOW);
    calDelay(80);
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(30, LCDV_MID);
    M5.Lcd.print(" Cal-1  ");
    gyroYoffset = 0.0;
    for (int i = 0; i < N_CAL1; i++) {
        readGyro();
        gyroYoffset += gyroYdata;
        delay(9);
    }
    gyroYoffset /= (float)N_CAL1;
    M5.Lcd.fillScreen(BLACK);
    digitalWrite(LED, HIGH);
}

void calib2() {
    resetVar();
    resetMotor();
    digitalWrite(LED, LOW);
    calDelay(80);
    M5.Lcd.setCursor(30, LCDV_MID);
    M5.Lcd.println(" Cal-2  ");
    accXoffset  = 0.0;
    gyroZoffset = 0.0;
    for (int i = 0; i < N_CAL2; i++) {
        readGyro();
        accXoffset += accXdata;
        gyroZoffset += gyroZdata;
        delay(9);
    }
    accXoffset /= (float)N_CAL2;
    gyroZoffset /= (float)N_CAL2;
    M5.Lcd.fillScreen(BLACK);
    digitalWrite(LED, HIGH);
}

void checkButtonP() {
    byte pbtn = M5.Axp.GetBtnPress();
    if (pbtn == 2)
        calib1();  // short push
    else if (pbtn == 1)
        setMode(true);  // long push
}

void calDelay(int n) {
    for (int i = 0; i < n; i++) {
        getGyro();
        delay(9);
    }
}

void setMode(bool inc) {
    if (inc) demoMode = ++demoMode % 2;
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(30, 5);
    if (demoMode == 0)
        M5.Lcd.print("Stand ");
    else if (demoMode == 1)
        M5.Lcd.print("Demo ");
}

void startDemo() {
    moveRate       = 1.0;
    spinContinuous = true;
    spinStep       = -40.0 * clk;
}

void resetPara() {
    Kang          = 37.0;
    Komg          = 0.84;
    KIang         = 800.0;
    Kyaw          = 4.0;
    Kdst          = 85.0;
    Kspd          = 2.7;
    mechFactL     = 0.45;
    mechFactR     = 0.45;
    punchPwr      = 20;
    punchDur      = 1;
    fbBalance     = -3;
    motorDeadband = 10;
    maxPwr        = 120;
    punchPwr2     = max(punchPwr, motorDeadband);
}

void getGyro() {
    readGyro();
    varOmg = (gyroYdata - gyroYoffset);           // unit:deg/sec
    yawAngle += (gyroZdata - gyroZoffset) * clk;  // unit:g
    varAng += (varOmg + ((accXdata - accXoffset) * 57.3 - varAng) * cutoff) *
              clk;  // complementary filter
}

void readGyro() {
    float gX, gY, gZ, aX, aY, aZ;
    M5.Imu.getGyroData(&gX, &gY, &gZ);
    M5.Imu.getAccelData(&aX, &aY, &aZ);
    gyroYdata = gX;
    gyroZdata = -gY;
    gyroXdata = -gZ;
    accXdata  = aZ;
    accZdata  = aY;
}

void drive() {
#ifdef DEBUG
    debugDrive();
#endif
    if (abs(moveRate) > 0.1)
        spinFact = constrain(-(powerR + powerL) / 10.0, -1.0, 1.0);  // moving
    else
        spinFact = 1.0;  // standing
    if (spinContinuous)
        spinTarget += spinStep * spinFact;
    else {
        if (spinTarget < spinDest) spinTarget += spinStep;
        if (spinTarget > spinDest) spinTarget -= spinStep;
    }
    moveTarget += moveStep * (moveRate + (float)fbBalance / 100.0);
    varSpd += power * clk;
    varDst += Kdst * (varSpd * clk - moveTarget);
    varIang += KIang * varAng * clk;
    power =
        varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
    if (abs(power) > 1000.0)
        counterOverPwr += 1;
    else
        counterOverPwr = 0;
    if (counterOverPwr > maxOvp) return;
    power    = constrain(power, -maxPwr, maxPwr);
    yawPower = (yawAngle - spinTarget) * Kyaw;
    powerR   = power - yawPower;
    powerL   = power + yawPower;

    ipowerL      = (int16_t)constrain(powerL * mechFactL, -maxPwr, maxPwr);
    int16_t mdbn = -motorDeadband;
    int16_t pp2n = -punchPwr2;
    if (ipowerL > 0) {
        if (motorLdir == 1)
            punchCountL = constrain(++punchCountL, 0, 100);
        else
            punchCountL = 0;
        motorLdir = 1;
        if (punchCountL < punchDur)
            drvMotorL(max(ipowerL, punchPwr2));
        else
            drvMotorL(max(ipowerL, motorDeadband));
    } else if (ipowerL < 0) {
        if (motorLdir == -1)
            punchCountL = constrain(++punchCountL, 0, 100);
        else
            punchCountL = 0;
        motorLdir = -1;
        if (punchCountL < punchDur)
            drvMotorL(min(ipowerL, pp2n));
        else
            drvMotorL(min(ipowerL, mdbn));
    } else {
        drvMotorL(0);
        motorLdir = 0;
    }

    ipowerR = (int16_t)constrain(powerR * mechFactR, -maxPwr, maxPwr);
    if (ipowerR > 0) {
        if (motorRdir == 1)
            punchCountR = constrain(++punchCountR, 0, 100);
        else
            punchCountR = 0;
        motorRdir = 1;
        if (punchCountR < punchDur)
            drvMotorR(max(ipowerR, punchPwr2));
        else
            drvMotorR(max(ipowerR, motorDeadband));
    } else if (ipowerR < 0) {
        if (motorRdir == -1)
            punchCountR = constrain(++punchCountR, 0, 100);
        else
            punchCountR = 0;
        motorRdir = -1;
        if (punchCountR < punchDur)
            drvMotorR(min(ipowerR, pp2n));
        else
            drvMotorR(min(ipowerR, mdbn));
    } else {
        drvMotorR(0);
        motorRdir = 0;
    }
}

void drvMotorL(int16_t pwm) {
    drvMotor(0, (int8_t)constrain(pwm, -127, 127));
}

void drvMotorR(int16_t pwm) {
    drvMotor(1, (int8_t)constrain(-pwm, -127, 127));
}

void drvMotor(byte ch, int8_t sp) {
    Wire.beginTransmission(0x38);
    Wire.write(ch);
    Wire.write(sp);
    Wire.endTransmission();
}

void resetMotor() {
    drvMotorR(0);
    drvMotorL(0);
    counterOverPwr = 0;
}

void resetVar() {
    power          = 0.0;
    moveTarget     = 0.0;
    moveRate       = 0.0;
    spinContinuous = false;
    spinDest       = 0.0;
    spinTarget     = 0.0;
    spinStep       = 0.0;
    yawAngle       = 0.0;
    varAng         = 0.0;
    varOmg         = 0.0;
    varDst         = 0.0;
    varSpd         = 0.0;
    varIang        = 0.0;
}

void sendStatus() {
    Serial.print(millis() - time0);
    Serial.print(" stand=");
    Serial.print(standing);
    Serial.print(" accX=");
    Serial.print(accXdata);
    Serial.print(" power=");
    Serial.print(power);
    Serial.print(" ang=");
    Serial.print(varAng);
    Serial.print(", ");
    Serial.print(millis() - time0);
    Serial.println();
}

void imuInit() {
    M5.Imu.Init();
    // if (M5.Imu.imuType = M5.Imu.IMU_MPU6886) {
    //     M5.Mpu6886.SetGyroFsr(
    //         M5.Mpu6886.GFS_250DPS);  // 250DPS 500DPS 1000DPS 2000DPS
    //     M5.Mpu6886.SetAccelFsr(M5.Mpu6886.AFS_4G);  // 2G 4G 8G 16G
    //     if (serialMonitor) Serial.println("MPU6886 found");
    // } else if (serialMonitor)
    //     Serial.println("MPU6886 not found");

    M5.Imu.SetGyroFsr(
    M5.Imu.GFS_250DPS);  // 250DPS 500DPS 1000DPS 2000DPS
    M5.Imu.SetAccelFsr(M5.Imu.AFS_4G);  // 2G 4G 8G 16G
    if (serialMonitor) Serial.println("MPU6886 found");
}

void dispBatVolt() {
    M5.Lcd.setCursor(35, LCDV_MID);
    vBatt = M5.Axp.GetBatVoltage();
    M5.Lcd.printf("%4.2fv ", vBatt);
}

A short video of it sort of working.  The calibration procedure is a bit flakey.  I need to take a look at the code to see how it works, but that's too much thinking for now...

Edit media

Dimensions x Small Medium Large Custom

Subject (required) Brief Description Tags (separated by comma) Video visibility in search results Visible Hidden

Parent content

BalaC Plus Balancing Robot

Poster

Upload Preview

Ok

Cancel

So, a nice diversion.  I'll need to figure out how to do something useful with it.  I'd like to add a time-of-flight sensor for obstacle detection.  I like that the main controller plugs into a connector.  I have other small controllers with BLE and IMUs that I'd like to try in place of the M5StickC Plus (Arduino Nano 33 BLE Sense and Seeed Xiao BLE Sense).  Maybe even try a Pi Pico W when that becomes available, although I'll have to add an IMU.