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