Self-balancing robot version 3 update!

It works! Now it balances on hard or soft floors for more than 30 minutes!

What changed?

Firstly, 2xAA rechargeable Eneloop batteries are insufficient to power those FS90R motors. The specification of the motor allows up to 6V. Two cells provide only around 2.7V total. I switch to 4xAA Eneloop batteries, providing around 5.4V. The motors were much much faster, more than twice as fast. The immediate consequence of this is that the PID parameters need to be re-tuned and scaled down to 10-20% of the values for 2xAA batteries.

Secondly, I turned up the builtin digital low pass filter in the MPU-9250 aggressively. Now it is set to 5 for both accelerometer and gyroscope. That translates to a sensor delay of 17ms and the 3dB bandwidth of about 10Hz. That is probably averaging around 17 samples at 1kHz sample rate. And that did the trick!

Here's a graph of motor power, gyroscope angular velocity and tilt angle.

The filtered angular velocity seldom exceeds 0.3 degrees per second. The filtered tilt angle seldom exceeds 1 degree. The motor power is mostly less than 10% of maximum power.

I've updated the InvenSense MPU-9255/MPU-9250/MPU-6500 module. You can now specify the low pass filter level for the accelerometer and gyroscope in the call to InvMPU.reset_mpu(). Copy this new version into your custom.ts.

The only changes from the version 1 code are the PID tuning parameters and the call InvMPU.reset_mpu(5, 5).

See the previous post on the parts for building version 3.

Self-balancing robot version 3

Version 1 was made out of Lego. It keeps breaking into pieces, the motors were not secured and it also contained the unnecessary Kitronic servo:lite board. So, I rebuilt it.

Parts:

• BBC micro:bit
• 2x Feetech FS90R 360 degrees servo motors
• InvenSense MPU-9255 9-axis gyroscope, accelerometer, compass.
• Pololu Wheel for FEETECH FS90R/FT90R Micro Servo 60x8mm
• 2x AAA battery holder to power the micro:bit
• 2x AA or 4x AA battery holder to power the motors
• 3D printed frame
• In place of wires, a little custom perfboard with connectors
• Bolts, spacers

The code for this version is identical to the code for version 1 except the tweek for a different orientation of the MPU-9255. With 2x AA batteries, identical PID parameters will work. With 4x AA batteries, the parameters have to be retuned.

But, alas, it still doesn't balance well on smooth hard floors!

It works fine on carpet or cushion even though it is only half the height of the version 1 robot. It just won't work on wood or marble floors!

After much tuning, I decided to record the motor response. I added two more wheels and turned the bot into a "car". Then I recorded the horizontal acceleration using the accelerometer in response to various motor control like a 1 second 66% power pulse. Here is a typical result with 2x AA batteries:

Why is the acceleration oscillating around zero? The amplitudes of the oscillations are much worse with 4x AA batteries.

Oh, the motor controller in the servo is a closed loop controller!

The closed-loop controller inside the FS90R is probably a variant of the standard PID controller. The oscillations are caused by the controller trying to match the output voltage to the pulse duration.

The accelerometer and gyroscope are picking up physical feedback from the controller's actions. On soft floor like carpet, the feedback is dampened physically. On hard floors, the sensors pick up random spikes that throws the PID algorithm off.

On top of that, the accelerometer only picks up the increase in motor speed after about 60ms!

In short, don't build a self-balancing robot using servo motors. Just use DC motors.

Update: I've fixed the problem and got it to work on hard floors too!

Trig module for micro:bit

I've updated the Trig module for micro:bit.

Now includes atan2(), sin(), cos(), rotate2d() and test code for the trigonometry functions.

Just cut and paste these into your custom.ts in MakeCode and you can skip the test code (control.assert).

/**
 * Public domain. Use at your own risk!
 * Trigonometry functions
 */
//% 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
        control.assert(y <= 32768 && y >= -32768, "atan2: y must be between -32768 and 32768: " + y)
        control.assert(x <= 32768 && x >= -32768, "atan2: x must be between -32768 and 32768: " + x)
        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;
    }
    control.assert(atan2(0, 0) == 0, "bad atan2(0, 0) = " + atan2(0, 0));
    control.assert(atan2(1, 0) == 9000, "bad atan2(1, 0) = " + atan2(1, 0));
    control.assert(atan2(-1, 0) == -9000, "bad atan2(-1, 0) = " + atan2(-1, 0));
    control.assert(atan2(1, 1) == 4500, "bad atan2(1, 1) = " + atan2(1, 1));
    control.assert(atan2(-1, 1) == -4500, "bad atan2(-1, 1) = " + atan2(-1, 1));
    control.assert(atan2(1, -1) == 13500, "bad atan2(1, -1) = " + atan2(1, -1));
    control.assert(atan2(-1, -1) == -13500, "bad atan2(-1, -1) = " + atan2(1, 1));
    control.assert(atan2(1, 2) == 2656, "bad atan2(1, 2) = " + atan2(1, 2));
    control.assert(atan2(-1, 2) == -2656, "bad atan2(-1, 2) = " + atan2(-1, 2));
    control.assert(atan2(1, -2) == 15344, "bad atan2(1, -2) = " + atan2(1, -2));
    control.assert(atan2(-1, -2) == -15344, "bad atan2(-1, -2) = " + atan2(1, -2));
    control.assert(atan2(572, 1) == 8990, "bad atan2(572, 1) = " + atan2(572, 1));

    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 {
        control.assert(angle >= -18000 && angle <= 18000, "angle must be netween -18000 and 18000: " + angle);
        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;
        }
    }

    control.assert(sin(0) == 0, "bad sin(0) = " + sin(0));
    control.assert(sin(3000) == 16384, "bad sin(3000) = " + sin(3000));
    control.assert(sin(6000) == 28378, "bad sin(6000) = " + sin(6000));
    control.assert(sin(9000) == 32768, "bad sin(9000) = " + sin(9000));
    control.assert(sin(12000) == 28378, "bad sin(12000) = " + sin(12000));
    control.assert(sin(15000) == 16384, "bad sin(15000) = " + sin(15000));
    control.assert(sin(18000) == 0, "bad sin(18000) = " + sin(18000));
    control.assert(sin(-3000) == -16384, "bad sin(-3000) = " + sin(-3000));
    control.assert(sin(-6000) == -28378, "bad sin(-6000) = " + sin(-6000));
    control.assert(sin(-9000) == -32768, "bad sin(-9000) = " + sin(-9000));
    control.assert(sin(-12000) == -28378, "bad sin(-12000) = " + sin(-12000));
    control.assert(sin(-15000) == -16384, "bad sin(-15000) = " + sin(-15000));
    control.assert(sin(-18000) == 0, "bad sin(-18000) = " + sin(-18000));
    control.assert(sin(10) == 57, "bad sin(10) = " + sin(10));
    control.assert(sin(20) == 114, "bad sin(20) = " + sin(20));
    control.assert(sin(30) == 172, "bad sin(30) = " + sin(30));
    control.assert(sin(40) == 229, "bad sin(40) = " + sin(40));
    control.assert(sin(50) == 286, "bad sin(50) = " + sin(50));
    control.assert(sin(60) == 343, "bad sin(60) = " + sin(60));
    control.assert(sin(70) == 400, "bad sin(70) = " + sin(70));
    control.assert(sin(80) == 458, "bad sin(80) = " + sin(80));
    control.assert(sin(90) == 515, "bad sin(90) = " + sin(90));
    control.assert(sin(-10) == -57, "bad sin(-10) = " + sin(-10));
    control.assert(sin(-20) == -114, "bad sin(-20) = " + sin(-20));
    control.assert(sin(-30) == -172, "bad sin(-30) = " + sin(-30));
    control.assert(sin(-40) == -229, "bad sin(-40) = " + sin(-40));
    control.assert(sin(-50) == -286, "bad sin(-50) = " + sin(-50));
    control.assert(sin(-60) == -343, "bad sin(-60) = " + sin(-60));
    control.assert(sin(-70) == -400, "bad sin(-70) = " + sin(-70));
    control.assert(sin(-80) == -458, "bad sin(-80) = " + sin(-80));
    control.assert(sin(-90) == -515, "bad sin(-90) = " + sin(-90));
    control.assert(sin(3000) == 16384, "bad sin(3000) = " + sin(3000));
    control.assert(sin(3010) == 16433, "bad sin(3010) = " + sin(3010)); // should really be 16434
    control.assert(sin(3020) == 16482, "bad sin(3020) = " + sin(3020)); // should really be 16483
    control.assert(sin(3030) == 16532, "bad sin(3030) = " + sin(3030));
    control.assert(sin(3040) == 16581, "bad sin(3040) = " + sin(3040)); // should really be 16582
    control.assert(sin(3050) == 16631, "bad sin(3050) = " + sin(3050));
    control.assert(sin(3060) == 16680, "bad sin(3060) = " + sin(3060));
    control.assert(sin(3070) == 16729, "bad sin(3070) = " + sin(3070));
    control.assert(sin(3080) == 16779, "bad sin(3080) = " + sin(3080));
    control.assert(sin(3090) == 16828, "bad sin(3090) = " + sin(3090));
    control.assert(sin(-3000) == -16384, "bad sin(-3000) = " + sin(-3000));
    control.assert(sin(-3010) == -16434, "bad sin(-3010) = " + sin(-3010));
    control.assert(sin(-3020) == -16483, "bad sin(-3020) = " + sin(-3020));
    control.assert(sin(-3030) == -16533, "bad sin(-3030) = " + sin(-3030)); // should really be -16532
    control.assert(sin(-3040) == -16582, "bad sin(-3040) = " + sin(-3040));
    control.assert(sin(-3050) == -16632, "bad sin(-3050) = " + sin(-3050)); // should really be -16631
    control.assert(sin(-3060) == -16681, "bad sin(-3060) = " + sin(-3060)); // should really be -16680
    control.assert(sin(-3070) == -16730, "bad sin(-3070) = " + sin(-3070)); // should really be -16729
    control.assert(sin(-3080) == -16780, "bad sin(-3080) = " + sin(-3080)); // should really be -16779
    control.assert(sin(-3090) == -16829, "bad sin(-3090) = " + sin(-3090)); // should really by -16828

    /**
     * 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);
        }
    }
    control.assert(cos(0) == 32768, "bad cos(0) = " + cos(0));
    control.assert(cos(3000) == 28378, "bad cos(000) = " + cos(3000));
    control.assert(cos(6000) == 16384, "bad cos(6000) = " + cos(6000));
    control.assert(cos(9000) == 0, "bad cos(9000) = " + cos(9000));
    control.assert(cos(12000) == -16384, "bad cos(12000) = " + cos(12000));
    control.assert(cos(15000) == -28378, "bad cos(15000) = " + cos(15000));
    control.assert(cos(18000) == -32768, "bad cos(18000) = " + cos(18000));
    control.assert(cos(-3000) == 28378, "bad cos(-3000) = " + cos(-3000));
    control.assert(cos(-6000) == 16384, "bad cos(-6000) = " + cos(-6000));
    control.assert(cos(-9000) == 0, "bad cos(-9000) = " + cos(-9000));
    control.assert(cos(-12000) == -16384, "bad sin(-12000) = " + sin(-12000));
    control.assert(cos(-15000) == -28378, "bad sin(-15000) = " + sin(-15000));
    control.assert(cos(-18000) == -32768, "bad sin(-18000) = " + sin(-18000));

    /**
     * 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);
    control.assert(t[0] == -30000 && t[1] == 20000, "After rotate 90 wrong: " + t[0] + ", " + t[1]);
    rotate2d(-9000, t);
    control.assert(t[0] == 20000 && t[1] == 30000, "After rotate -90 wrong: " + t[0] + ", " + t[1]);
    rotate2d(4500, t);
    control.assert(t[0] == -7071 && t[1] == 35354, "After rotate 45 wrong: " + t[0] + ", " + t[1]);
    rotate2d(-4500, t);
    control.assert(t[0] == 19998 && t[1] == 29998, "After rotate -45 wrong: " + t[0] + ", " + t[1]);
}

Microbit serial to file script

How do you save thousands of lines from the micro:bit connected via the serial port to a file?

screen? Cut and paste from the terminal?

That gets tiring after a while. Furthermore, the name of the serial device on OS X keeps changing.

Well, here's a handy little script. It looks for the first connected micro:bit and then copies the serial output to stdout. You can tee or pipe it to a file.

Requires pyserial. Only tested on OS X. As usual, use at your own risk.

import serial
from serial.tools.list_ports import comports as list_serial_ports
import sys

def get_microbit_port(ser_id):
    ports = list_serial_ports()
    if ser_id != "": 
        for port in ports:
            if ser_id in port[2]:
                return port[0]
        return None
    for port in ports:
        if "VID:PID=0D28:0204" in port[2]:
            return port[0]
    return None

def main():
    id = ""
    if len(sys.argv) > 1:
        id = sys.argv[1]
    port = get_microbit_port(id)
    if port == None:
        print("micro:bit not connected")
        sys.exit(1)

    try:
        ser = serial.Serial(port, baudrate=115200)
        while True:
            line = ser.readline()
            print line,
            sys.stdout.flush()
    except serial.SerialException as e:
        print "Serial line disconnected"
    except KeyboardInterrupt:
        print "Quit"
    finally:
        try:
            ser.close()
        except:
            pass

main()

Micro-bit logging module

One more utility package.

When trying to balance the robot, it is hard to know what is going on without logging the numbers from every iteration of the control loop. However, there is no straightforward way to log 6 data points 100 times per seconds for 10 seconds. Bluetooth and radio are too slow and lossy. Attaching a serial line affects the balance of the robot. Try the simple way of adding numbers to an array and it runs out of memory quickly.

Well, there's an undocumented Buffer class that can store an array of bytes. This Logging class is a wrapper around that class. It provides a simple interface to repeatedly log a line consisting of a fixed number of numbers. In the constructor, specify the number of lines to keep and a list of byte sizes, one for each number. Then call add() to log a line of data. If the logger runs out of space, it'll wraparound and throw away the oldest data. Finally, call the sendToSerial() method to write the buffer to the serial line in human readable format. Each call to add() becomes a line of numbers separated by space.

There is enough memory for about 8000 bytes.

Add this to the end of your custom.ts.

/**
 * Public domain. Use at your own risk!
 * Logging functions
 */
namespace Log {
    export class Log {
        private line_count: number;
        private labels: Array;
        private sizes: Array;
        private line_size: number;
        private buf_size: number;
        private buf: Buffer;
        private tail: number;
        private full: boolean;

        /**
         * Creates a logging object
         * @param line_count Number of lines, eg: 1000
         * @param sizes Array of number byte size per line, eg [4, 1, 1, 2]
         */
        constructor(line_count: number, labels: Array, sizes: Array) {
            this.line_count = line_count;
            this.labels = labels;
            this.sizes = sizes;
            control.assert(this.labels.length == this.sizes.length);
            this.line_size = 0;
            for (let i = 0; i < sizes.length; i++) {
                let s = sizes[i];
                control.assert(s == 1 || s == 2 || s == 4);
                this.line_size = this.line_size + s;
            }
            this.buf_size = this.line_size * this.line_count;
            this.buf = pins.createBuffer(this.buf_size);
            this.tail = 0;
            this.full = false;
        }

        /**Adds the list of numbers to the log according to the byte sizes array in the constructor.
         * @param l List of numbers to be added
         */
        add(l: Array) {
            let p = this.tail * this.line_size;
            for (let i = 0; i < this.sizes.length; i++) {
                let s = this.sizes[i];
                let n = l[i];
                switch (s) {
                    case 1:
                        this.buf.setNumber(NumberFormat.Int8LE, p, n);
                        break;
                    case 2:
                        this.buf.setNumber(NumberFormat.Int16LE, p, n);
                        break;
                    case 4:
                        this.buf.setNumber(NumberFormat.Int32LE, p, n);
                        break;
                }
                p = p + s;
            }
            control.assert(p == (this.tail + 1) * this.line_size);
            this.tail = this.tail + 1;
            if (this.tail >= this.line_count) {
                this.tail = 0;
                this.full = true;
            }
        }

        clear() {
            this.tail = 0;
            this.full = false;
        }

        //%
        sendToSerial() {
            let start = 0;
            let count = this.tail;
            if (this.full) {
                count = this.line_count;
                start = this.tail;
            }
            for (let i = 0; i < this.labels.length; i++) {
                serial.writeString(this.labels[i]);
                if (i == this.labels.length - 1) {
                    serial.writeLine("");
                } else {
                    serial.writeString(",");
                }
            }
            for (let i = 0; i < count; i++) {
                this.sendLine((i + start) % this.line_count);
            }
        }

        private sendLine(index: number) {
            let p = index * this.line_size;
            for (let i = 0; i < this.sizes.length; i++) {
                let s = this.sizes[i];
                switch (s) {
                    case 1:
                        serial.writeNumber(this.buf.getNumber(NumberFormat.Int8LE, p));
                        break;
                    case 2:
                        serial.writeNumber(this.buf.getNumber(NumberFormat.Int16LE, p));
                        break;
                    case 4:
                        serial.writeNumber(this.buf.getNumber(NumberFormat.Int32LE, p));
                        break;
                }
                p = p + s;
                if (i == this.sizes.length - 1) {
                    serial.writeLine("");
                } else {
                    serial.writeString(",");
                }
            }
        }
    }
}

Example code:

// Each line is consists of 3 numbers: [4 bytes, 1 byte, 2 bytes]
let l = new Log.Log(1000, ["time", "delta", "x"], [4, 1, 2])

input.onButtonPressed(Button.A, () => {
    serial.writeLine("START");
    l.sendToSerial();
    serial.writeLine("END");
})

basic.showIcon(IconNames.Heart)
let last = input.runningTime();
let x = 0;
while (true) {
    let t = input.runningTime();
    l.add([t, t - last, x]);
    last = t;
    x = x + 1;
    basic.pause(1);
}

Code for self-balancing micro:bit robot with DC motors

The DC motors version of the code is similar to the servo version except for the motor control code and the tuning parameters.

This works with version 2 of the self-balancing robot.

See the code for version 1 for more info.

/**
 * Public domain. Use at your own risk!
 * Self-balancing robot controller, controlling a pair of FS90R servos.
 * Requires InvMPU and Trig package in custom.ts.
 * Update read_gyro_angle_rate() and read_accel_tilt_angle() to sensor mounting.
 * Update call to InvMPU.set_gyro_bias() with the bias of your sensor.
 */

// Tuning Parameters
const TARGET_ANGLE = -560; // forward tilt from vertical in degrees * 100
const KE = 2400;
const KD = 24;
const KI = 30;

const motor_bias = 0; // increase to reduce forward power to the right

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

// Left motor connected with L293D
const motor_a_enable = AnalogPin.P13; // enable 1-2
const motor_a1 = DigitalPin.P2; // input 1
const motor_a2 = DigitalPin.P12; // input 2

// Right motor connect with L293D
const motor_b_enable = AnalogPin.P14; // enable 3-4
const motor_b1 = DigitalPin.P15; // input 3
const motor_b2 = DigitalPin.P16; // input 4

function motor_coast() {
    pins.analogWritePin(motor_a_enable, 0);
    pins.digitalWritePin(motor_a1, 0);
    pins.digitalWritePin(motor_a2, 0);
    pins.analogWritePin(motor_b_enable, 0);
    pins.digitalWritePin(motor_b1, 0);
    pins.digitalWritePin(motor_b2, 0);
}

function motor_move(left: number, right: number) {
    if (left >= 0) {
        pins.analogWritePin(motor_a_enable, left);
        pins.digitalWritePin(motor_a1, 1);
        pins.digitalWritePin(motor_a2, 0);
    } else {
        pins.analogWritePin(motor_a_enable, -left);
        pins.digitalWritePin(motor_a1, 0);
        pins.digitalWritePin(motor_a2, 1);
    }
    if (right >= 0) {
        pins.analogWritePin(motor_b_enable, right);
        pins.digitalWritePin(motor_b1, 1);
        pins.digitalWritePin(motor_b2, 0);
    } else {
        pins.analogWritePin(motor_b_enable, -right);
        pins.digitalWritePin(motor_b1, 0);
        pins.digitalWritePin(motor_b2, 1);
    }
}

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(5, 1, -13); // 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);
        if (motor_on && (est_angle > 3000 || est_angle < -3000)) {
            last_err = 0;
            i_err = 0;
            motor_coast();
            motor_on = false;
        }
        let err = est_angle - TARGET_ANGLE;
        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 * KE + d_err * KD + i_err * KI;
            let motor_out = u / 1000;
            let motor_right = motor_out - motor_bias;
            let motor_left = motor_out + motor_bias;
            motor_move(Math.clamp(-1024, 1023, motor_left), Math.clamp(-1024, 1023, motor_right));
        } else if (err <= 500 && err >= -500) {
            motor_on = true;
        }
        basic.pause(5);
    }
}

setup();
control_loop();

Code for self-balancing micro:bit robot with FS90R servo motors

Ok, here's the code for version 1 of the self-balancing robot. I tried to make this work in block mode in MakeCode. The result was unreadable.

Only 80 lines of code. It implements the most primitive PID control algorithm. Clearly Segway and other self-balancing single wheel devices use far better algorithms.

To use this, you neeed to:

• Build a robot with one of three InvenSense motion sensors and FS90R or other servo motors.
• Add the InvMPU and Trig packages from the last post to custom.ts.
• Copy and paste this code in main.ts.
• Set the gyro bias using the values computed in the sample program from the last post.
• Change read_gyro_angle_rate() and read_accel_tilt_angle() based on how the sensor is mounted on the robot.
• Tune TARGET_ANGLE, KE, KD and KI.

When the happy face appears, press the A button to locate the sensor and start the bot. When the bot is near vertical, it will turn on the motors to balance. If the angle is too far off vertical, it will shut down the motors. Hold it vertical to start balancing again.

Here it is:

/**
 * Public domain. Use at your own risk!
 * Self-balancing robot controller, controlling a pair of FS90R servos.
 * Requires InvMPU and Trig package in custom.ts.
 * Update read_gyro_angle_rate() and read_accel_tilt_angle() to sensor mounting.
 * Update call to InvMPU.set_gyro_bias() with the bias of your sensor.
 */

// Tuning Parameters
const TARGET_ANGLE = 50; // forward tilt from vertical in degrees * 100
const KE = 1200;
const KD = 24;
const KI = 24;

const motor_bias = 0; // increase to reduce forward power to the right

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 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(5, 1, -13); // 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);
        if (motor_on && (est_angle > 3000 || est_angle < -3000)) {
            last_err = 0;
            i_err = 0;
            pins.digitalWritePin(DigitalPin.P15, 0);
            pins.digitalWritePin(DigitalPin.P16, 0);
            motor_on = false;
        }
        let err = est_angle - TARGET_ANGLE;
        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 * KE + d_err * KD + i_err * KI;
            let motor_out = u / 10000;
            pins.servoWritePin(AnalogPin.P15, 90 - Math.clamp(-90, 90, motor_out - motor_bias)); // R
            pins.servoWritePin(AnalogPin.P16, 90 + Math.clamp(-90, 90, motor_out + motor_bias)); // L
        } else if (err <= 500 && err >= -500) {
            motor_on = true;
        }
        basic.pause(5);
    }
}

setup();
control_loop();

See the next post for a version of this code that works with DC motors.

InvenSense MPU-9255/MPU-9250/MPU-6500 gyroscope accelerometer and compass

EDIT: Arduino version of here.

The BBC micro:bit does not have a builtin gyroscope and the self-balancing robot requires one.

Among the many options, the MPU-6500/MPU-9250/MPU-9255 based breakout boards are probably among the cheapest motion sensors available. The MPU-6500 costs SGD9.90 from a local retail shop and is available online for much less. The differences between the models are:

• MPU-6500: gyroscope and accelerometer, 6 axis.
• MPU-9250: gyroscope, accelerometer and compass, 9 axis
• MPU-9255: same as MPU-9250 (almost)

The gyroscope and accelerometer are identical across all three models. Each axis has 16 bits of resolution. The gyroscope measures up to +- 2000 degrees per second. The accelerometer measures up to +-16g. The builtin accelerometer on the micro:bit only has 11 bits of resolution and a range of +-4g.

There are no drivers or library for these chips for the micro:bit. InvenSense provides product specification and register map documents, enough to figure out how to interface with the chips. I can ignore the  advanced features like the FIFO buffer, interrupts and the digital motion processor. I just need to configure the chips, read the gyroscope and accelerometer values.

So, if you need a cheap gyroscope and accelerometer that works with micro:bit, here's a working library you can add to your code.

Hardware instructions

• Connect VCC and GND to the 3V and GND pins on the micro:bit respectively.
• Connect SCL and SDA to pin 19 and 20 on the micro:bit respectively. These are the default I2C pins.
• Connect AD0 to GND to select the default I2C address.

Software instructions

• Create a new project on MakeCode.
• Click on the arrow at the bottom left to show the simulator.
• Click "{} Javascript" to switch to Javascript mode.
• Click on "Explorer" to expand the Explorer tree.
• Click on the "+" on the Explorer bar to add a custom.ts script.
• Click "Go ahead!" on the "Add custom blocks?" prompt.
• Now, it should be showing the custom.ts script. Delete all the default code.

If your project already has a custom.ts script, ignore the above instructions. Just switch to that script.

Now copy and append the code below to the end of your custom.ts.

Update: reset_mpu() now takes two parameters gyro_lpf and accel_lpf to control the digital low pass filter.

/**
 * 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);
    }
}

Switch back to main.ts and reload your project. Your main code can now access blocks and javascript that talks to the motion sensor chips.

The InvMPU package contains these functions and they are also available as blocks from the blocks interface of MakeCode:

• find_mpu(): find one of the three MPUs on the I2C bus and returns true if found.
• reset_mpu(): reset the MPU and configure it to +-16g and 2000 degrees per second.
• compute_gyro_bias(): compute the gyroscope bias. The sensor must be stationary.
• set_gyro_bias(): set the gyroscope bias to values computed in previous calls in get_gyro_bias().
• read_gyro(): read all three gyroscope axis values.
• read_accel(): read all three accelerometer values.

One more thing, we really want to know the angle the sensor or robot is tilted from vertical. The axis parallel to the wheels is fixed. We can calculate the angle from the other two axes of the accelerometer using the inverse tangent function atan2(). Unfortunately, the micro:bit does not provide this function or support floating point numbers.

No worries. Here my little approximation of atan2() that takes two signed 16 bits values from the accelerometer and returns 100 times the angle in degrees. For example, if Trig.atan2() returns 2050, it means the angle is 20.5 degrees.

Copy and append the Trig module for micro:bit to the end of your custom.ts.

Update Jan 2021: MakeCode v3 now supports trigonometry functions. Instead of Trig.atan2, use Math.atan2(), convert to degrees and round the result to an integer.

Here's sample code that finds the mpu, resets it, computes the gyro bias, sets the gyro bias and prints values from the gyroscope and accelerometer and the tilt angle on the serial line.

BTW, the bias values are reasonably stable for each hardware breakout board. I use the bias values calculated by the sample program in the balancing code so that I don't have to recompute the bias each time I turn the robot on.

let angle = 0
basic.showIcon(IconNames.Happy)
while (true) {
    while (!(input.buttonIsPressed(Button.A))) {
        basic.pause(100)
    }
    basic.showIcon(IconNames.Diamond)
    if (!(InvMPU.find_mpu())) {
        if (InvMPU.sensor_id != 0) {
            serial.writeLine("Unknown sensor id " + InvMPU.sensor_id)
        }
        serial.writeLine("Cannot find MPU-6500, MPU-6500 or MPU-9255")
        basic.showIcon(IconNames.No)
    } else {
        if (InvMPU.sensor_id == InvMPU.MPU_6500_ID) {
            serial.writeLine("MPU-6500")
        }
        if (InvMPU.sensor_id == InvMPU.MPU_9250_ID) {
            serial.writeLine("MPU-9250")
        }
        if (InvMPU.sensor_id == InvMPU.MPU_9255_ID) {
            serial.writeLine("MPU-9255")
        }
        InvMPU.reset_mpu()
        basic.pause(2000)
        basic.clearScreen()
        if (InvMPU.compute_gyro_bias()) {
            serial.writeLine("X variance " + InvMPU.var_x)
            serial.writeLine("Y variance " + InvMPU.var_y)
            serial.writeLine("Z variance " + InvMPU.var_z)
            serial.writeLine("X bias " + InvMPU.gyro_x_bias)
            serial.writeLine("Y bias " + InvMPU.gyro_y_bias)
            serial.writeLine("Z bias " + InvMPU.gyro_z_bias)
            InvMPU.set_gyro_bias(InvMPU.gyro_x_bias, InvMPU.gyro_y_bias, InvMPU.gyro_z_bias)
            break;
        } else {
            basic.showIcon(IconNames.Angry)
        }
    }
}
basic.showIcon(IconNames.Yes)
while (true) {
    InvMPU.read_gyro()
    InvMPU.read_accel()
    angle = Trig.atan2(InvMPU.accel_z, 0 - InvMPU.accel_x)
    serial.writeLine("Gyro: " + InvMPU.gyro_x + " " + InvMPU.gyro_y + " " + InvMPU.gyro_z + " Angle: " + angle + " Accel: " + InvMPU.accel_x + " " + InvMPU.accel_y + " " + InvMPU.accel_z)
    basic.pause(100)
}

Coming up, code for the self-balancing robot.

Public domain. 

Use at your own risk!

Self-balancing robot version 2

Every project must include something new.

I wasn't happy with version 1.

Here's version 2, completely different:

Parts:

• BBC micro:bit
• Kitronic micro:bit edge connector breakout board
• Blue plastic parts, hobby DC motors and wheels from a Dagu Magician Chassis.
• 2xAAA battery holder for the micro:bit
• 4xAA battery holder for the DC motors
• Homemade L293D motor controller board
• InvenSense MPU-9255 9-axis gyroscope, accelerometer, compass.
• Wires, spacers, bolts, nuts

It stays up for over half an hour on smooth floors! It also doesn't roam around as much as version 1.

View from the top.

The big differences between version 2 and version 1 are:

• rigid frame and motor mounts instead of Lego pieces
• hobby DC motors instead servo motors
• two battery holders to power the motors separately from everything else
• L293D motor controller

The L293D is an ancient motor controller from many decades ago. It isn't energy efficient, runs hot and can only provide 600mA of sustained current per channel. However, that is more than sufficient because I don't need to run the motors at full power at all. Besides, L293D has separate power and motor pins, builtin clamping diodes to protect the chip from motor back emf and over-temperature protection.

The various data sheets (STmicroelectronics , Texas Instruments) have all the application information needed to use the chip. Even better, check out this guide.

First, I prototyped the circuit on the breadboard on the donor chassis:

Then I squeezed the DIP socket, capacitors, a switch and connectors on a tiny perfboard I happened to have. The two connectors on the right are for the two motors and the two connectors on the left are for motor and logic power. The three wires on each side together control a motor.

The back doesn't look so nice. And I really do not recommend using such a small perfboard.

The micro:bit works with 2V to 3.6V and I am powering it with 2.7V. The L293D needs at least 2.3V for high input and the micro:bit drives high outputs very close to its 2.7V supply voltage. That works. However, the L293D needs at least 4.5V for the logic power supply pin according to the data sheet. I should really use a 5V step-up regulator. But it seems to work with 2.7V and that will do for now. This is just a toy after all.

The code is mostly the same as version 1. Instead of using the servo apis to control the servo motor, each DC motor is controlled by applying PWM signals to the motor enable pin (grey line) and setting the correct output to two other lines (yellow and green).

Finally, the PID controller's parameters need to be retuned by hand.

Yeah, I was going to post the code.

But first, I have to talk about the 9-axis MPU-9255 gyroscope, accelerometer and compass that is used in both versions of the robot. Coming up.