Balancing Bot Update 2020

Since the last post three years ago, Micro:bit MakeCode has been updated.

The latest version is MakeCode v3 2020 Update. The balancing bot code was written using MakeCode v0. MakeCode v3 is not compatible with MakeCode v0.

The big incompatible change is that numbers in MakeCode v3 are now floating point while numbers in MakeCode v0 were 16 bit integers. MakeCode v3 also includes trigonometric functions in its library.

Here's the updated bot code that works with MakeCode v3. The Trig library should be removed from the custom.ts file. It is not needed anymore.

/**
 * Public domain. Use at your own risk!
 */
let TARGET_ANGLE = 155
let KE = 420
let KD = 8
let KI = 5
let motor_left_bias = -2;
let motor_right_bias = 2;
function control_loop() {
    motor_coast()
    let motor_out = 0;
    let vertical_count = 0;
    let motor_on = false
    let est_angle = read_accel_tilt_angle()
    let start_time = input.runningTime()
    let last_time = start_time
    let last_err = 0;
    let i_err = 0;
    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)) {
            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
            motor_out = Math.clamp(-90, 90, u / 10000)
            motor_move(motor_out + motor_left_bias, motor_out + motor_right_bias)
        } else if (err <= 500 && err >= -500) {
            vertical_count = vertical_count + 1
            if (vertical_count > 100) {
                vertical_count = 0
                last_err = 0
                i_err = 0
                est_angle = read_accel_tilt_angle()
                motor_on = true
            }   
        } else {
            vertical_count = 0
        }        
        basic.pause(1)
    }
}

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

function read_accel_tilt_angle(): number {
    InvMPU.read_accel()
    return Math.round(Math.atan2(InvMPU.accel_z, -InvMPU.accel_x) * 18000 / Math.PI)
}

function updateAngle(old_angle: number, delta_t_ms: number): number {
    let accel_angle = read_accel_tilt_angle()
    let gyro_angle_rate = read_gyro_angle_rate() * 200000 / 32768
    let gyro_angle_change = gyro_angle_rate * delta_t_ms / 1000
    return (49 * (old_angle + gyro_angle_change) + accel_angle) / 50
}

function motor_coast() {
    pins.digitalWritePin(DigitalPin.P15, 0)
    pins.digitalWritePin(DigitalPin.P16, 0)
}

function motor_move(left: number, right: number) {
    pins.servoWritePin(AnalogPin.P15, 90 - right)
    pins.servoWritePin(AnalogPin.P16, 90 + left)
}

input.onButtonPressed(Button.A, function on_button_pressed_a() {
    TARGET_ANGLE = TARGET_ANGLE + 5
})

input.onButtonPressed(Button.B, function on_button_pressed_b() {
    TARGET_ANGLE = TARGET_ANGLE - 5
})

input.onButtonPressed(Button.AB, function on_button_pressed_ab() {
    basic.showNumber(TARGET_ANGLE)
})

if (InvMPU.find_mpu()) {
    basic.showIcon(IconNames.Happy)
    InvMPU.reset_mpu(5, 5)
    basic.pause(100)
    InvMPU.set_gyro_bias(5, 1, -13)
    control_loop()
} else {
    basic.showIcon(IconNames.No)
}

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