Self-Balancing Lego Robot with Micro:bit & Gyroscope

This is a project to learn about how Self Balancing Robot works. A Self-Balancing Robot (in this case) has 2 wheels side by side and balance itself by moving the wheels based on a gyroscope sensor reading. I used a Micro:bit and Lego for this project.

What is Micro:bit?

The BBC Micro:bit is a single board computer similar to Arduino.
Unlike Arduino however, Micro:bit has some buttons, sensors, Bluetooth and LEDs already integrated on the board ready to be utilised.

Components

Motors

Gyroscope

Motor Controller

Mobile Battery

I used a USB mobile battery I had at the time to power the robot.

Script to Read from Gyroscope

I can control motors by the driver provided by Kitronik (above). I then need to read from Gyroscope.
I have referred to following pages on “pengtoh’s blog” below and added some minor modifications.

pengtoh’s blog – InvenSense MPU-9255/MPU-9250/MPU-6500 gyroscope accelerometer and compass

pengtoh’s blog – Trig module for micro:bit

In the MakeCode environment. Create a new project and switch to JavaScript mode. Then let the environment create a “custom.ts” by selecting [+] icon in Explorer menu.

You can replace the content of “custom.ts” with the script below.

/**
 * Public domain. Use at your own risk!
 * Simplified interface for InvenSense MPU-6500, MPU-9250, MPU-9255
 */
//% weight=90 color=#0040A0
namespace InvMPU {
    export const MPU_6500_ID = 0x70;
    export const MPU_9250_ID = 0x71;
    export const MPU_9255_ID = 0x73;

    const MPU_ADDR = 0x68;
    const WHO_AM_I = 0x75;
    const REG_PWR_MGMT_1 = 0x6b;
    const REG_SIGNAL_PATH_RESET = 0x68;
    const REG_USER_CTRL = 0x6a;
    const REG_ACCEL_XOUT_H = 0x3b;
    const REG_ACCEL_YOUT_H = 0x3d;
    const REG_ACCEL_ZOUT_H = 0x3f;
    const REG_GYRO_XOUT_H = 0x43;
    const REG_GYRO_YOUT_H = 0x45;
    const REG_GYRO_ZOUT_H = 0x47;
    const REG_CONFIG = 0x1a;
    const REG_GYRO_CONFIG = 0x1b;
    const REG_ACCEL_CONFIG = 0x1c;
    const REG_ACCEL_CONFIG2 = 0x1d;
    const XG_OFFSET_H = 0x13;
    const YG_OFFSET_H = 0x15;
    const ZG_OFFSET_H = 0x17;

    function mpu_read(reg: number): number {
        pins.i2cWriteNumber(MPU_ADDR, reg, NumberFormat.UInt8BE, true);
        return pins.i2cReadNumber(MPU_ADDR, NumberFormat.UInt8BE, false);
    }

    function mpu_read_int16(reg: number): number {
        pins.i2cWriteNumber(MPU_ADDR, reg, NumberFormat.UInt8BE, true);
        return pins.i2cReadNumber(MPU_ADDR, NumberFormat.Int16BE, false);
    }

    function mpu_write(reg: number, data: number) {
        pins.i2cWriteNumber(MPU_ADDR, reg << 8 | (data & 0xff), NumberFormat.UInt16BE);
    }

    function mpu_write_int16(reg: number, data: number) {
        mpu_write(reg, (data >> 8) & 0xff);
        mpu_write(reg + 1, data & 0xff);
    }

    /**
     * Contains one of MPU_6500_ID, MPU_9250_ID, MPU_9255_ID or zero.
     */
    //% block
    export let sensor_id = 0;

    /**
     * Look for a MPU-6500, MPU-9250 or MPU-9255.
     * Returns true if the MPU was found.
     * The MPU id is in sensor_id.
     */
    //% block
    //% weight=99
    export function find_mpu(): boolean {
        sensor_id = mpu_read(WHO_AM_I);
        return sensor_id == MPU_9255_ID || sensor_id == MPU_9250_ID || sensor_id == MPU_6500_ID;
    }

    /**
     * Reset the MPU and configure the gyroscope to +- 2000 degrees/second and the accelerometer to +-16g.
     * The low pass filter settings control how sensitive the sensors are to quick changes.
     * In order of increasing sensitivity: 6, 5, 4, 3, 2, 1, 0, 7
     * Info from MPU-9250 register map document.
     * @param gyro_lpf Gyroscope low pass filter setting, eg: 0
     *   7: 8kHz sampling rate, 36001Hz bandwidth, 0.17ms delay.
     *   0: 8kHz sampling rate, 250Hz bandwidth, 0.97ms delay.
     *   1: 1kHz sampling rate, 184Hz bandwidth, 2.9ms delay.
     *   2: 1kHz sampling rate, 92Hz bandwidth, 3.9ms delay.
     *   3: 1kHz sampling rate, 41Hz bandwidth, 5.9ms delay.
     *   4: 1kHz sampling rate, 20Hz bandwidth, 9.9ms delay.
     *   5: 1kHz sampling rate, 10Hz bandwidth, 17.85ms delay.
     *   6: 1kHz sampling rate, 5Hz bandwidth, 33.48ms delay.
     * @param accel_lpf Accelerometer low pass filter setting, eg: 0
     *   7: 1kHz sampling rate, 420Hz 3dB bandwidth, 1.38ms delay.
     *   0: 1kHz sampling rate, 218.1Hz 3dB bandwidth, 1.88ms delay.
     *   1: 1kHz sampling rate, 218.1Hz 3dB bandwidth, 1.88ms delay.
     *   2: 1kHz sampling rate, 99Hz 3dB bandwidth, 2.88ms delay.
     *   3: 1kHz sampling rate, 44.8Hz 3dB bandwidth, 4.88ms delay.
     *   4: 1kHz sampling rate, 21.2Hz 3dB bandwidth, 8.87ms delay.
     *   5: 1kHz sampling rate, 10.2Hz 3dB bandwidth, 16.83ms delay.
     *   6: 1kHz sampling rate, 5.05Hz 3dB bandwidth, 32.48ms delay.
     */
    //% block
    //% weight=98
    export function reset_mpu(gyro_lpf = 0, accel_lpf = 0) {
        control.assert(gyro_lpf >= 0 || gyro_lpf <= 7, "gyro_lpf must be between 0 and 7");
        control.assert(accel_lpf >= 0 || accel_lpf <= 7, "accel_lpf must be between 0 and 7");
        mpu_write(REG_PWR_MGMT_1, 0x80); // H_RESET, internal 20MHz clock
        mpu_write(REG_SIGNAL_PATH_RESET, 0x7); // GYRO_RST | ACCEL_RST | TEMP_RST
        mpu_write(REG_USER_CTRL, 0x1); // SIG_COND_RST
        mpu_write(REG_CONFIG, gyro_lpf);
        mpu_write(REG_GYRO_CONFIG, 0x18); // GYRO_FS_SEL = 3, +-2000 dps, DLPF on
        mpu_write(REG_ACCEL_CONFIG, 0x18); // ACCEL_FS_SEL = 3, +-16g
        mpu_write(REG_ACCEL_CONFIG2, accel_lpf); // DLPF on
    }

    /**
     * Gyro X axis value after calling read_gyro().
     * Value from -32768 to 32767.
     */
    //% block
    export let gyro_x = 0;

    /**
     * Gyro Y axis value after calling read_gyro().
     * Value from -32768 to 32767.
     */
    //% block
    export let gyro_y = 0;

    /**
     * Gyro Z axis value after calling read_gyro().
     * Value from -32768 to 32767.
     */
    //% block
    export let gyro_z = 0;

    /**
     * Read all three gyroscope axis values and store them in gyro_x, gyro_y and gyro_z.
     */
    //% block
    //% weight=95
    export function read_gyro() {
        gyro_x = mpu_read_int16(REG_GYRO_XOUT_H);
        gyro_y = mpu_read_int16(REG_GYRO_YOUT_H);
        gyro_z = mpu_read_int16(REG_GYRO_ZOUT_H);
    }

    /**
     * Accelerometer X axis value after calling read_accel().
     * Value from -32768 to 32767.
     */
    //% block
    export let accel_x = 0;

    /**
     * Accelerometer Y axis value after calling read_accel().
     * Value from -32768 to 32767.
     */
    //% block
    export let accel_y = 0;

    /**
     * Accelerometer Z axis value after calling read_accel().
     * Value from -32768 to 32767.
     */
    //% block
    export let accel_z = 0;

    /**
     * Read all three accelerometer values and store them in accel_x, accel_y and accel_z.
     */
    //% block
    //% weight=94
    export function read_accel() {
        accel_x = mpu_read_int16(REG_ACCEL_XOUT_H);
        accel_y = mpu_read_int16(REG_ACCEL_YOUT_H);
        accel_z = mpu_read_int16(REG_ACCEL_ZOUT_H);
    }

    export let var_x = 0;
    export let var_y = 0;
    export let var_z = 0;

    /**
     * Gyro X axis bias after calling get_gyro_bias().
     */
    //% block
    export let gyro_x_bias = 0;

    /**
     * Gyro Y axis bias after calling get_gyro_bias().
     */
    //% block
    export let gyro_y_bias = 0;

    /**
     * Gyro Z axis bias after calling get_gyro_bias().
     */
    //% block
    export let gyro_z_bias = 0;

    /**
     * Compute the gyro bias for all three axis. The bias for each axis is the average of 100 samples.
     * Returns true if the sensor is steady enough to calculate the bias.
     * The bias values are store in gyro_x_bias, gyro_y_bias and gyro_z_bias.
     */
    //% block
    //% weight=97
    export function compute_gyro_bias(): boolean {
        mpu_write_int16(XG_OFFSET_H, 0);
        mpu_write_int16(YG_OFFSET_H, 0);
        mpu_write_int16(ZG_OFFSET_H, 0);
        const N = 100;
        const MAX_VAR = 40;
        let sum_x = 0, sum_y = 0, sum_z = 0;
        let xs: number[] = [], ys: number[] = [], zs: number[] = [];
        for (let i = 0; i < N; i++) {
            let x = mpu_read_int16(REG_GYRO_XOUT_H);
            let y = mpu_read_int16(REG_GYRO_YOUT_H);
            let z = mpu_read_int16(REG_GYRO_ZOUT_H);
            sum_x += x;
            sum_y += y;
            sum_z += z;
            xs.push(x);
            ys.push(y);
            zs.push(z);
            basic.pause(1);
        }
        let mean_x = sum_x / N;
        let mean_y = sum_y / N;
        let mean_z = sum_z / N;
        var_x = 0;
        var_y = 0;
        var_z = 0;
        for (let i = 0; i < N; i++) {
            let dx = xs[i] - mean_x;
            var_x = var_x + dx * dx;
            let dy = ys[i] - mean_y;
            var_y = var_y + dy * dy;
            let dz = zs[i] - mean_z;
            var_z = var_z + dz * dz;
        }
        var_x = var_x / N;
        var_y = var_y / N;
        var_z = var_z / N;
        if (var_x > MAX_VAR || var_y > MAX_VAR || var_z > MAX_VAR) {
            return false;
        }
        gyro_x_bias = mean_x;
        gyro_y_bias = mean_y;
        gyro_z_bias = mean_z;
        return true;
    }

    /**
     * Set the gyro bias. The bias can be calculated by calling get_gyro_bias().
     * @param x_bias Bias in the X direction, eg: 0
     * @param y_bias Bias in the Y direction, eg: 0
     * @param z_bias Bias in the Z direction, eg: 0
     */
    //% block
    //% weight=96
    export function set_gyro_bias(x_bias: number, y_bias: number, z_bias: number) {
        mpu_write_int16(XG_OFFSET_H, -2 * x_bias);
        mpu_write_int16(YG_OFFSET_H, -2 * y_bias);
        mpu_write_int16(ZG_OFFSET_H, -2 * z_bias);
    }
}


//% weight=90 color=#00A040
namespace Trig {
    const atan_table: number[] = [
        0, 1144, 2289, 3435, 4583, 5734, 6889, 8047, 9211, 10380, // 0
        11556, 12739, 13931, 15131, 16340, 17561, 18793, 20037, 21294, 22566, // 10
        23854, 25157, 26479, 27819, 29179, 30560, 31965, 33393, 34847, 36328, // 20
        37838, 39379, 40952, 42560, 44205, 45889, 47615, 49385, 51203, 53071, // 30
        54992, 56970, 59009, 61114, 63288, 65536, 67865, 70279, 72786, 75391, // 40
        78103, 80931, 83883, 86970, 90203, 93596, 97162, 100917, 104880, 109071, // 50
        113512, 118231, 123256, 128622, 134369, 140543, 147197, 154394, 162208, 170728, // 60
        180059, 190331, 201700, 214359, 228552, 244584, 262851, 283868, 308323, 337154, // 70
        371674, 413779, 466313, 533748, 623534, 749080, 937209, 1250502, 1876706, 3754555, // 80
        37549324, // 89.9 approx 90
    ];

    /**
     * Returns the inverse tangent of y/x in degrees * 100.
     * @param y Number between -32768 and 32768, eg: 2000
     * @param x Number between -32768 and 32768, eg: -1000
     */
    //% block
    //% weight=100
    export function atan2(y: number, x: number): number {
        // returns degrees * 100    
        if (x == 0) {
            if (y == 0) {
                return 0;
            } else if (y > 0) {
                return 9000;
            } else {
                return -9000;
            }
        }
        let ratio = (y << 16) / x;
        let sign = 1;
        if (ratio < 0) {
            sign = -1;
            ratio = - ratio;
        }
        for (let i = 1; i < atan_table.length; i++) {
            if (ratio < atan_table[i]) {
                let d = atan_table[i] - atan_table[i - 1];
                let d2 = ratio - atan_table[i - 1];
                let d3 = d2 > 21474836 ? d2 * 10 / d * 10 : d2 * 100 / d;
                if (x < 0) {
                    return sign * ((i - 1) * 100 + d3 - 18000);
                } else {
                    return sign * ((i - 1) * 100 + d3);
                }
            }
        }
        return sign * 9000;
    }

    const sin_table: number[] = [
        0, 572, 1144, 1715, 2286, 2856, 3425, 3993, 4560, 5126, // 0
        5690, 6252, 6813, 7371, 7927, 8481, 9032, 9580, 10126, 10668, // 10
        11207, 11743, 12275, 12803, 13328, 13848, 14365, 14876, 15384, 15886, // 20
        16384, 16877, 17364, 17847, 18324, 18795, 19261, 19720, 20174, 20622, // 30
        21063, 21498, 21926, 22348, 22763, 23170, 23571, 23965, 24351, 24730, // 40
        25102, 25466, 25822, 26170, 26510, 26842, 27166, 27482, 27789, 28088, // 50
        28378, 28660, 28932, 29197, 29452, 29698, 29935, 30163, 30382, 30592, // 60
        30792, 30983, 31164, 31336, 31499, 31651, 31795, 31928, 32052, 32166, // 70
        32270, 32365, 32449, 32524, 32588, 32643, 32688, 32723, 32748, 32763, // 80
        32768,
    ];

    function sin_deg(d: number): number {
        if (d >= 0 && d <= 90) {
            return sin_table[d];
        } else if (d > 90 && d <= 180) {
            return sin_table[180 - d];
        } else if (d < 0 && d >= -90) {
            return -sin_table[-d];
        } else {
            return -sin_table[180 + d];
        }
    }
    function cos_deg(angle: number): number {
        if (angle >= 0) {
            return sin_deg(90 - angle);
        } else {
            return sin_deg(90 + angle);
        }
    }
    function sin_small(x: number): number {
        return [0, 57, 114, 172, 229, 286, 343, 400, 458, 515][x];
    }
    function cos_small(x: number): number {
        return [32768, 32768, 32768, 32768, 32767, 32767, 32766, 32766, 32765, 32764][x];
    }

    /**
     * Returns 32768 * sin of the angle.
     * @param angle Degrees * 100, between -18000 and 18000, eg: 9000
     */
    //% block
    //% weight=90
    export function sin(angle: number): number {

        if (angle < 0) { // microbit rounds towards 0
            let z = (-angle + 5) / 10;
            let r = z % 10;
            let d = z / 10;
            return -(sin_deg(d) * cos_small(r) + cos_deg(d) * sin_small(r)) >> 15;
        } else {
            let z = (angle + 5) / 10;
            let r = z % 10;
            let d = z / 10;
            return (sin_deg(d) * cos_small(r) + cos_deg(d) * sin_small(r)) >> 15;
        }
    }

    /**
     * Returns 32768 * cos of the angle.
     * @param angle Degrees * 100, between -18000 and 18000, eg: 9000
     */
    //% block
    //% weight=89
    export function cos(angle: number): number {
        if (angle >= 0) {
            return sin(9000 - angle);
        } else {
            return sin(9000 + angle);
        }
    }

    /**
     * Rotates a vector [x, y] by angle degrees anti-clockwise and updates it in place.
     * @param angle Degrees * 100, between -18000 and 18000, eg: 9000
     * @param v Vector represemted as an array [x, y]
     */
    //% block
    //% weight=80
    export function rotate2d(angle: number, v: number[]) {
        let c = cos(angle);
        let s = sin(angle);
        let v0 = (c * v[0] - s * v[1]) >> 15;
        let v1 = (s * v[0] + c * v[1]) >> 15;
        v[0] = v0;
        v[1] = v1;
    }
    let t: number[] = [20000, 30000];
    rotate2d(9000, t);
    rotate2d(-9000, t);
    rotate2d(4500, t);
    rotate2d(-4500, t);
}

Script to Balance the Robot

The concept behind the script to make the robot balancing itself is interesting. There are 3 different personalities debating next move and the programmer configure opinions between those personalities.

I did “try & error” to find a good enough combination, but there should be a way to calculate near-perfect configuration.
If you are perfectionist, you might need to measure battery voltage to adjust the motor speed as well.

You can go back to “main.ts” and paste the script below.
The first 7 lines should be adjusted per different robot to make it self-balanced.

// Tuning Parameters
const TARGET_ANGLE = -8600; // forward tilt from vertical in degrees * 100
const MOTOR = 250 // less = strong
const KP = 400; //Realist - Correct only current error
const KI = 3; //Pessimist - Correct accumulated error
const KD = 11;　//Flip-flopper - Correct difference between current & last error
const motor_bias = -5; // increase to reduce forward power to the left

function read_gyro_angle_rate(): number {
    InvMPU.read_gyro()
    return -InvMPU.gyro_y
}

function read_accel_tilt_angle(): number {
    InvMPU.read_accel()
    return Trig.atan2(-InvMPU.accel_z, InvMPU.accel_x); // degrees * 100
}

function updateAngle(est_angle: number, delta_t_ms: number): number {
    let accel_angle = read_accel_tilt_angle(); // degrees * 100
    let gyro_angle_rate = read_gyro_angle_rate() * 200000 / 32768; // (degrees * 100) per second
    let gyro_angle_change = gyro_angle_rate * delta_t_ms / 1000; // degrees * 100
    let new_est_angle = (49 * (est_angle + gyro_angle_change) + accel_angle) / 50;
    return new_est_angle;
}

function motor_coast() {
    kitronik_motor_driver.motorOn(kitronik_motor_driver.Motors.Motor1, kitronik_motor_driver.MotorDirection.Forward, 0);
    kitronik_motor_driver.motorOn(kitronik_motor_driver.Motors.Motor2, kitronik_motor_driver.MotorDirection.Forward, 0);
}

function motor_move(left: number, right: number) {
    if (left >= 0) {
        kitronik_motor_driver.motorOn(kitronik_motor_driver.Motors.Motor1, kitronik_motor_driver.MotorDirection.Forward, Math.abs(left));
    } else {
        kitronik_motor_driver.motorOn(kitronik_motor_driver.Motors.Motor1, kitronik_motor_driver.MotorDirection.Reverse, Math.abs(left));
    }
    if (right >= 0) {
        kitronik_motor_driver.motorOn(kitronik_motor_driver.Motors.Motor2, kitronik_motor_driver.MotorDirection.Forward, Math.abs(right));
    } else {
        kitronik_motor_driver.motorOn(kitronik_motor_driver.Motors.Motor2, kitronik_motor_driver.MotorDirection.Reverse, Math.abs(right));
    }
}

function setup() {
    basic.showIcon(IconNames.Happy);
    while (true) {
        while (!input.buttonIsPressed(Button.A)) {

            basic.pause(10);
        }
        if (InvMPU.find_mpu()) {
            break;
        }
        basic.showIcon(IconNames.No);
    }
    InvMPU.reset_mpu();
    basic.pause(100);
    basic.clearScreen();
    InvMPU.set_gyro_bias(0, 0, 0); // set this for your sensor
}


function control_loop() {
    let motor_on = false;
    let est_angle = read_accel_tilt_angle(); // degrees * 100 from vertical
    let last_err = 0;
    let i_err = 0;
    let last_time = input.runningTime();
    while (true) {
        let current_time = input.runningTime();
        let delta_t = current_time - last_time;
        last_time = current_time;
        est_angle = updateAngle(est_angle, delta_t);

        let err = est_angle - TARGET_ANGLE;
        if (motor_on && (err > 3000 || err < -3000)) {
            last_err = 0;
            i_err = 0;
            motor_coast();
            motor_on = false;
        }
        if (motor_on) {
            let d_err = (err - last_err) * 1000 / delta_t;
            last_err = err;
            i_err = i_err + err * delta_t;
            let u = err * KP + d_err * KD + i_err * KI;
            let motor_out = u / 3000;
            let motor_right = motor_out - motor_bias;
            let motor_left = motor_out + motor_bias;
            motor_move(pins.map(motor_left, -MOTOR, MOTOR, -100, 100), pins.map(motor_right, -MOTOR, MOTOR, -100, 100));
        } else if (err <= 500 && err >= -500) {
            motor_on = true;
        }
        basic.pause(5);
    }
}

setup();
control_loop();