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