76行代碼如何完成一個雙輪平衡小車？

Ruff Lite

Ruff Lite 是 Ruff 團隊針對 MCU（MicroController Unit，微控制器）推出的 Ruff OS
，具備高實時性，佔用內存小等特色。目前官方支持的開發板爲TI TM4C1294-LaunchPad ，Ruff Lite支持的硬件接口包括：GPIO、UART、I2C、ADC、PWM、QEI。

原理簡介

兩輪自動平衡車是一個典型的自動控制系統，由執行元件（直流電機），傳感模塊（陀螺儀和編碼器）和主控平臺控制系統（Ruff 開發板）組成。直流電機控制兩輪正反轉，陀螺儀檢測車身姿態，編碼器檢測電機轉速，這兩組傳感器數據反饋給控制系統，經由 PID 控制算法計算，給出控制直流電機的控制量，經過這一閉環過程，從而造成負反饋，保證車身平衡

物件清單

主控平臺

Ruff Lite 開發版 （型號 TM4C1294-V1 ）

傳感器及執行元件

• 陀螺儀模塊 （型號 GY-521）

• 直流電機驅動模塊 （型號 TB6612FNG ）

• 編碼器模塊（隨直流電機一體）（型號 MG513-30 ）

MG513-30 是該直流電機的型號，由電機驅動模塊進行驅動，這裏咱們用它自帶的編碼器模塊

其它

• 機械元件

• 12V 鋰電池

• 電壓轉換模塊（12V-5V）

直流電機 12V 供電，開發板 5V 供電

開發步驟

1. 初始化 APP，選擇 tm4c1294-v1 開發板（對應 TI TM4C1294-LaunchPad）

$ rap init --board tm4c1294-v1

2. 添加陀螺儀驅動，id 爲 gyro，型號選擇 GY-521，其他參數默認

$ rap device add gyro (GY-521)

3. 添加電機驅動，id 爲 motor，型號選擇 TB6612FNG

$ rap device add motor (TB6612FNG)

4. 添加編碼器驅動，id 爲 encoder，型號選擇 MG513-30，其他參數默認

$ rap device add encoder (MG513-30)

5. 編寫控制算法

（見下文）

6. 調試

（見下文）

7. 掃描開發板

$ rap scan

7. 部署應用

$ rap deploy

控制算法

PID（比例-積分-微分）控制算法是工程上最經常使用的自動控制算法，參數 P 實現基本控制做用，參數 D 避免系統震盪，參數I用來消除系統靜差，本平衡小車系統由 PID 算法進行控制，從而保持平衡

平衡車內部有兩個反饋環，一個是由陀螺儀反饋姿態傾角和角速度構成的 直立環，一個是由編碼器反饋直流電機轉速構成的 速度環，由此構成一個串級 PID 控制系統（見下圖），速度環控制的輸出做爲直立環控制的輸入，直立環由 PD 控制（比例-微分控制）系統構成，保證小車的基本平衡（參數 P 的做用）和避免震盪（參數 D 的做用），速度環由 PI 控制（比例-積分控制）系統，消除姿態傾角的靜差（參數I的做用）

具體 PID 算法的原理及推導過程請參考自控控制專業書籍，這裏只對算法做用和各個參數的意義進行簡要說明

控制程序

src/index.js

$.ready(function(error) {
    if (error) {
        console.log('error', error);
        return;
    }

    var gyro = $('#gyro'); // 陀螺儀傳感器（GY-521）
    var enc = $('#encoder'); // 編碼器傳感器（MG513-30）
    var motor = $('#motor'); // 電機驅動控制器（TB6612FNG）

    // 直立環PD控制
    var getBalancePwm = function (actualAngle, actualGyro) {
        var targetAngle = 0.8; // 小車靜止平衡時的姿態角度
        var kP = 80; // 直立環比例(P)控制參數
        var kD = 2; // 直立環微分(D)控制參數

        // 直立環控制份量
        var balancePwm = kP * (actualAngle - targetAngle) + kD * actualGyro;
        return balancePwm / 1000;
    };

    // 速度環PI控制
    var encoder = 0;
    var sumEncoder = 0;
    var getVelocityPwm = function (actualLEncoder, actualREncoder) {
        var targetVelocity = 0; // 小車靜止平衡時的電機輸出轉速
        var kP = 3; // 速度環比例(P)控制參數
        var kI = kP / 200; // 速度環積分(I)控制參數

        // FIR二階低通濾波
        encoder = 0.2 * (actualLEncoder + actualREncoder - targetVelocity) + 0.8 * encoder;
        sumEncoder += encoder;

        // 積分限幅
        if (sumEncoder >= 3000) {
            sumEncoder = 3000;
        }
        if (sumEncoder <= -3000) {
            sumEncoder = -3000;
        }

        // 速度換控制份量
        var velocityPwm = kP * encoder + kI * sumEncoder;
        return (velocityPwm / 1000);
    };

    var cycle = 20; // 採樣/控制週期均爲20ms，即1s採樣/控制50次
    var gyroAcquire, encAcquire, balanceControl;

    var angleX = 0;
    var gyroY = 0;
    var rpm = 0;

    // 每隔20ms，獲取陀螺儀沿X軸的姿態傾角和角速度
    gyroAcquire = setInterval(function() {
        gyro.getFusedMotionX(cycle, function (error, _angleX, _gyroY) {
            angleX = _angleX;
            gyroY = _gyroY;
        });
    }, cycle);

    // 每隔20ms，獲取編碼器的速度值
    encAcquire = setInterval(function() {
        enc.getRpm(function (error, _rpm) {
            rpm = _rpm;
        });
    }, cycle);

    // 每隔20ms，利用反饋值計算控制量，控制電機正反轉
    balanceControl = setInterval(function() {
        var balancePwm = getBalancePwm(angleX, gyroY);
        var velocityPwm = getVelocityPwm(rpm, rpm);
        var pwmDuty = balancePwm - velocityPwm;

        if (pwmDuty >= 0) {
            if (pwmDuty >= 1) {
                pwmDuty = 1;
            }
            // 控制車身前進（電機A正轉B反轉，A與B相差180度安裝）
            motor.forwardRotateA(pwmDuty);
            motor.backwardRotateB(pwmDuty);
        } else {
            if (pwmDuty <= -1) {
                pwmDuty = -1;
            }
            // 控制車身後退（電機A反轉B正轉，A與B相差180度安裝）
            motor.backwardRotateA(-pwmDuty);
            motor.forwardRotateB(-pwmDuty);
        }

        // 若傾角超過30度，中止整個控制系統運行
        if (angleX >= 30 || angleX <= -30) {
            // 中止陀螺儀採樣
            clearInterval(gyroAcquire);
            // 中止編碼器採樣
            clearInterval(encAcquire);
            // 中止電機控制邏輯
            clearInterval(balanceControl);
            // 中止電機A/B轉動
            motor.stopRotateA();
            motor.stopRotateB();
        }
    }, cycle);
});

調試

目標角度調試

裝好整個機械元件後，要進行目標角度調試，即 targetAngle 變量，具體方法，將控制 motor 先後轉動的代碼所有註釋掉，而後在 balanceControl 這個函數中，打印 angleX，獲得小車趨於平衡靜止時的角度，應該大約在正負3度之內。

算法參數調試

首先肯定參數的極性。

先屏蔽掉外反饋環 PI 控制，保持 kP 和 kD 參數不變，看是否小車有平衡的趨勢，及車輪是否會向傾倒的一側轉動，如果，則 kP 和 kD 參數爲正數不須要改變，若否，則 kP 和 kI 參數須要改成負數。

以後屏蔽掉內反饋環 PD 控制，保持 kI 和 kD 參數不變，用手去轉動鏈接編碼器的那個車輪，看是不是此PI控制是正反饋，即給車輪一個小的轉動，車輪是否會一直加速到最大速度，如果，則 kP 和 kD 參數爲正數不須要改變，若否，則 kP 和 kI 參數須要改成負數（上述程序中只須要改 kP 便可，kI 爲 kP/200）。

而後肯定參數的數值。通常狀況下，整個機械元件裝穩定後，PD 算法參數（kP 和 kD）和 PI 算法中的參數（kP 和 kI）應該不須要變更就能夠直接運行在你的平衡小車上。

FAQ

Ruff MPU版（ruff-mbd-v1）能夠做爲主控平臺麼？

不能，由於底層的 OpenWRT（基於 Linux）不是實時操做系統，系統啓動後會運行不少進程，Ruff 進程不必定時刻佔有 CPU，所以不能穩定地每隔一個控制週期（這裏是 20ms）得到傳感器數據，不知足控制系統的實時性要求。而 MCU 版 Ruff 的底層操做系統是 Nuttx RTOS，可以保證明時操做。

控制系統中控制週期是多少？

控制週期爲 20ms，即1秒內控制系統控制 50 次。每一個控制週期須要作的內容包括 1) 獲取陀螺儀和編碼器兩個傳感器的數據，2) 傳入直立環和速度環算法中進行計算獲得控制量，3) 將控制量做用於直流電機上。

用Ruff MCU開發板開發平衡車與用其它開發板（如 stm32）進行裸板開發，有什麼相同點與不一樣點？

相同點是都知足實時性控制的要求（如本案例的 20ms 控制週期）。

不一樣點主要體如今 開發效率和 可移植性兩個方面，若用其它 MCU 開發板進行裸板開發，要面對 硬件接口協議（I2C接口陀螺儀，QEI接口編碼器，PWM 和 GPIO 接口的直流電機控制器），外設模塊協議（好比給陀螺儀發送什麼命令獲取到加速度值和角速度值）和 硬件定時器中斷（經過配置寄存器設置20ms定時器中斷）等其它問題，且代碼不具有可移植性和複用性，但在整個開發過程當中若用 Ruff 開發，可直面業務邏輯，即自動控制算法，而不用關心硬件模塊的任何細節，你面對的只有外設模塊的 API，而且因爲沒有任何硬件平臺的邏輯，程序自己具有可複用性。