Balancing Bot Update 2020

EDIT: See 2024 update and Arduino version!

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