Self Balancing Robot

In this second tutorial we’ll be creating a simulation of a self balancing robot using a cascaded PID control loop. After completing this tutorial, you should be more familiar with configuring components and writing custom scripts. Start by downloading the CAD geometry for the model.

Solution

The completed model can be downloaded.

Inner Control Loop

The inner control loop balances the robot by trying to achieve a set target angle. We use the built-in PID type to do this. Proportional-Integral-Derivative (PID) controllers take an input (current) value and a setpoint (target) value. They produce a response from the difference (error) between the current and setpoint values.

The response is calculated as the weighted sum of three terms:

1. Proportional: Term that is proportional to the error.
2. Integral: Term that is proportional to the integral (sum) of the error over time.
3. Derivative: Term that is proportional to the rate of change of the error.

The kp, ki, and kd constants are the weights for each of these terms respectively.

The complete source code for the inner control loop is provided below.

import { Component, type Entity, type Handle, MotorComponent, Units, UnitType, PID, Util, Vec3 } from "prototwin";

export class Controller extends Component {
    public motor1: Handle<MotorComponent>;
    public motor2: Handle<MotorComponent>;

    @Units(UnitType.AngularVelocity)
    public maxSpeed: number;

    #balancer: PID;

    constructor(entity: Entity) {
        super(entity);
        this.motor1 = this.handle(MotorComponent);
        this.motor2 = this.handle(MotorComponent);
        this.maxSpeed = Util.radians(4000);
        this.#balancer = new PID();
        this.#balancer.kp = 1000;
        this.#balancer.ki = 0;
        this.#balancer.kd = 100;
    }

    public override update(dt: number): void {
        const targetAngle = Util.radians(5);
        this.#balancer.setpoint = targetAngle;
        const angle = Vec3.signedAngleUnit(this.entity.worldRotation.zaxis, Vec3.y, Vec3.x);
        const velocity = Util.clamp(this.#balancer.evaluate(angle, dt), -this.maxSpeed, this.maxSpeed);
        this.motor1.value!.targetVelocity = velocity;
        this.motor2.value!.targetVelocity = velocity;
    }
}

The response of the PID controller is an angular velocity, which we pass to the motors as their target velocities. However, we first clamp the response so that the magnitude of the velocity doesn’t exceed the maximum speed.

Note that the current value for the PID controller is calculated by measuring the angle between the robot’s z-axis (in world-space) and the global y-axis. Since we need to know the direction that the robot is tilted, we must calculate this as a signed angle. We use the built-in function Vec3.signedAngleUnit(start, end, n) to measure the angle from the start vector to the end vector in the plane with the normal vetor n. ProtoTwin uses a right-handed coordinate system, so angles are measured counter-clockwise when looking against the direction of the normal.

Outer Control Loop

The final step is to add an outer PID control loop, which controls the position of the robot. The setpoint for this outer loop is the target position that we want to move to. The current value is the robot’s z-coordinate (in world-space). The response of the outer PID control loop is the target angle, which we now feed in as the inner loop’s setpoint. However, we first clamp this response to avoid tilting the robot beyond a threshold angle.

The complete source code for the component is provided below.

import { Component, type Entity, type Handle, MotorComponent, Units, UnitType, PID, Util, Vec3 } from "prototwin";

export class Controller extends Component {
    public motor1: Handle<MotorComponent>;
    public motor2: Handle<MotorComponent>;

    @Units(UnitType.AngularVelocity)
    public maxSpeed: number;

    #balancer: PID;
    #positioner: PID;

    public get targetPosition(): number {
        return this.#positioner.setpoint;
    }

    public set targetPosition(value: number) {
        this.#positioner.setpoint = value;
    }

    constructor(entity: Entity) {
        super(entity);
        this.motor1 = this.handle(MotorComponent);
        this.motor2 = this.handle(MotorComponent);
        this.maxSpeed = Util.radians(4000);
        this.#balancer = new PID();
        this.#positioner = new PID();
        this.#balancer.kp = 1000;
        this.#balancer.ki = 0;
        this.#balancer.kd = 100;
        this.#positioner.kp = 0.5;
        this.#positioner.ki = 0;
        this.#positioner.kd = 0.35;
    }

    public override update(dt: number): void {
        const position = this.entity.worldPosition.z;
        const targetAngle = this.#positioner.evaluate(position, dt);
        this.#balancer.setpoint = -Util.clamp(targetAngle, Util.radians(-8), Util.radians(8));
        const angle = Vec3.signedAngleUnit(this.entity.worldRotation.zaxis, Vec3.y, Vec3.x);
        const velocity = Util.clamp(this.#balancer.evaluate(angle, dt), -this.maxSpeed, this.maxSpeed);
        this.motor1.value!.targetVelocity = velocity;
        this.motor2.value!.targetVelocity = velocity;
    }
}