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）应该不需要变动就可以直接运行在你的平衡小车上。

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，并且由于没有任何硬件平台的逻辑，程序本身具备可复用性。