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.