开发者成长激励计划-基于TencentOS Tiny和CH32V307_EVB的星球大战BB-8机器人控制系统

原创

HonestQiao

修改于 2022-08-11 13:58:59

99800

代码可运行

文章被收录于专栏：Tencent Tiny

运行总次数：0

代码可运行

概述

背景

在星球大战原力觉醒中，出现了一位新的机器人BB-8，其形象如下：

该机器人，由迪士尼设计，Sphero负责具体实现，应用独有的技术，制作了一个真实的球形机器人BB-8，其球形主体可以自由移动，头部也可以跟着运动。

两者联合，又开发了玩具版版本，可以试用手机直接控制，也可以进行编程控制：

为了更好的氪金，他们居然又推出了原力手环，可以隔空操作BB-8：

虽然我在BB-8上市时，第一时间抢了一个，是在是太爱了。但是原力手环一个就要买800大洋，太不划算。

因为酷爱BB-8，所以对其进行了很多的技术研究。结合多方公布的资料，我已经试用nodejs实现了一个BB-8多功能控制系统，并且可以通过websocket或者mqtt，让其他的设备，也能够转发指令进行控制。

刚好借着这次使用TencentOS Tiny和CH32V307_EVB的机会，结合我之前的研究，实现一个能够隔空操纵BB-8的控制系统。

实现功能

在这个控制系统中，核心板试用CH32V307_EVB，运行TencentOS Tiny，通过I2C，从MPU6050运动传感器获取运动姿态信息，然后生成控制指令，通过mqtt发送控制指令，发送指令控制BB-8机器人。

在这个控制系统中，通过I2C获取传感器的数据，使用卡尔曼过滤来优化运动姿态数据，虽然目前用于控制BB-8，经过改造，也可以用于其他类似场合的控制。

系统框架

该系统框架的整体结构并不复杂，主要是读取运动数据，然后进过结算，最好生成控制指令，通过MQQ发送到BB-8控制转发系统，最终控制BB-8机器人。

硬件框架

硬件资源

该控制系统中，所使用的硬件设备如下：

CH32V307_EVB开发板
MPU6050传感器：DFRobot版本，连接到CH32V307_EVB开发板的E53对应I2C接口
BB-8机器人
4珠WS2812灯板：用于指示当前控制的方向【计划】
废弃儿童电动牙刷：掏空，放置MPU6050，便于操控

软件框架

软件资源

在该控制系统中，所使用的软件系统包括：

TencentOS Tiny，及官方提供的DFP演示代码，主要其中包含ESP8266联网和显示屏处理
MPU6050数据读取模块
MQTT通讯模块
EMQX：自建mqtt服务
BB-8网页控制系统：https://gitee.com/honestqiao/bb8
BB-8控制测试与转发系统：https://gitee.com/honestqiao/BB8_Proxy

软件模块说明

MPU6050数据解析模块

// MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt;                                                   //微分时间 A4连接SDA   A5连接SCL
int16_t ax, ay, az, gx, gy, gz;                             //加速度计陀螺仪原始数据  Typedef符号字符
float aax = 0, aay = 0, aaz = 0, agx = 0, agy = 0, agz = 0; //角度变量
long axo = 0, ayo = 0, azo = 0;                             //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0;                             //陀螺仪偏移量
float pi = 3.1415926;
float AcceRatio = 16384.0;                                                                      //加速度计比例系数
float GyroRatio = 131.0;                                                                        //陀螺仪比例系数
uint8_t n_sample = 8;                                                                           //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0};                                              // x,y轴采样队列
long aax_sum, aay_sum, aaz_sum;                                                                 // x,y轴采样和
float a_x[10] = {0}, a_y[10] = {0}, a_z[10] = {0}, g_x[10] = {0}, g_y[10] = {0}, g_z[10] = {0}; //加速度计协方差计算队列
float Px = 1, Rx, Kx, Sx, Vx, Qx;                                                               // x轴卡尔曼变量
float Py = 1, Ry, Ky, Sy, Vy, Qy;                                                               // y轴卡尔曼变量
float Pz = 1, Rz, Kz, Sz, Vz, Qz;                                                               // z轴卡尔曼变量
volatile int Accy;
volatile int Accx;

#define CENTER_X 150
#define CENTER_Y 150

static unsigned long millis()
{
    return (unsigned long)tos_millisec2tick(1);
}

void mpu6050_task_entry(void *arg)
{
    // u8 i=0;
	//float temperature, humidity;
	
	u16 ir, als, ps;

    short aacx,aacy,aacz;       //加速度原始数据
    short gyrox,gyroy,gyroz;    //陀螺仪原始数据
    short temp;                 //MPU-6050 温度

    // int i;
    // int n = 5;
    int d = 100;

	// Delay_Init();
	// USART_Printf_Init(115200);
	printf("SystemClk:%d\r\n",SystemCoreClock);

    // lcd_init();

    // lcd_set_color(BLACK,WHITE);
    // lcd_show_string(50, 0, 32,"openCH.io");
    // lcd_set_color(BLACK,RED);
    // lcd_show_string(0, 32, 16,"I2C Device Test");
    LCD_ShowString(30,140+16+16+16+16,"mpu6050 init",WHITE,BLACK,16,0);
    tos_task_delay(100);

    // 初值
    Accy = 0;
    Accx = 0;

    // lcd_set_color(BLACK,GREEN);
    // lcd_show_string(180, 48, 16,"OK");
    // lcd_set_color(BLACK,WHITE);
    // lcd_show_string(0, 112, 16,"MPU6050");
    while(MPU_Init())
    {
        // lcd_set_color(BLACK,RED);
        // lcd_show_string(180, 112, 16,"Error");
        LCD_ShowString(30,140+16+16+16+16,"Error",WHITE,BLACK,16,0);
        tos_task_delay(1000);
        // Delay_Ms(200);
        // lcd_show_string(180, 112, 16,"     ");
        LCD_ShowString(30,140+16+16+16+16,".....",WHITE,BLACK,16,0);
        tos_task_delay(1000);
        // Delay_Ms(200);
    }
    // lcd_set_color(BLACK,GREEN);
    // lcd_show_string(180, 112, 16,"OK");
    LCD_ShowString(30,140+16+16+16+16,"mpu6050 ok  ",WHITE,BLACK,16,0);
    tos_task_delay(1000);
    
    // 初始化数据
    unsigned short times = 200; //采样次数
    for (int i = 0; i < times; i++)
    {
        printf("read MPU6050 TEMP start\r\n");
        temp=MPU_Get_Temperature(); //获得温度
        printf("read MPU6050 Acc start\r\n");
        MPU_Get_Accelerometer(&aacx,&aacy,&aacz);   //获得加速度原始数据
        printf("read MPU6050 GRY start\r\n");
        MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //获得陀螺仪原始数据

        ax = (float)aacx;
        ay = (float)aacy;
        az = (float)aacy;

        gx = (float)gyrox;
        gy = (float)gyroy;
        gz = (float)gyroy;

        axo += ax;
        ayo += ay;
        azo += az; //采样和
        gxo += gx;
        gyo += gy;
        gzo += gz;
    }
    axo /= times;
    ayo /= times;
    azo /= times; //计算加速度计偏移
    gxo /= times;
    gyo /= times;
    gzo /= times; //计算陀螺仪偏移
    // 初始化数据 end

    LCD_Fill(0,0,LCD_W,LCD_H,BLACK);

    LCD_ShowString(30, 112-64,   "MPU6050   temp : ",WHITE,BLACK,16,0);
    LCD_ShowString(30, 128-64,  "X : ",WHITE,BLACK,16,0);
    LCD_ShowString(30, 144-64,  "Y : ",WHITE,BLACK,16,0);
    LCD_ShowString(30, 160-64,  "Z : ",WHITE,BLACK,16,0);
    LCD_ShowString(128, 128-64, "GX : ",WHITE,BLACK,16,0);
    LCD_ShowString(128, 144-64, "GY : ",WHITE,BLACK,16,0);
    LCD_ShowString(128, 160-64, "GZ : ",WHITE,BLACK,16,0);

    LCD_ShowString(30, 160+24-64,  "ACCX : ",WHITE,BLACK,16,0); 
    LCD_ShowString(128, 160+24-64, "ACCY : ",WHITE,BLACK,16,0);

	while(1)
	{
        printf("read MPU6050 TEMP start\r\n");
        temp=MPU_Get_Temperature(); //获得温度
        printf("read MPU6050 Acc start\r\n");
        MPU_Get_Accelerometer(&aacx,&aacy,&aacz);   //获得加速度原始数据
        printf("read MPU6050 GRY start\r\n");
        MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //获得陀螺仪原始数据
        // lcd_set_color(BLACK,WHITE);
        // lcd_show_string(0, 112, 16,"MPU6050   temp : %5d", temp);
        // lcd_set_color(BLACK,GREEN);
        // lcd_show_string(30, 128, 16,"X : %6d", aacx);
        // lcd_show_string(30, 144, 16,"Y : %6d", aacy);
        // lcd_show_string(30, 160, 16,"Z : %6d", aacz);
        // lcd_show_string(128, 128, 16,"GX : %6d", gyrox);
        // lcd_show_string(128, 144, 16,"GY : %6d", gyroy);
        // lcd_show_string(128, 160, 16,"GZ : %6d", gyroz);
        char strff[21],strdd[21];
        memset(strff,0,sizeof(strff));sprintf(strff,"%5d",temp);
        LCD_ShowString(30+128, 112-64,   strff, WHITE,BLACK,16,0);
        memset(strff,0,sizeof(strff));sprintf(strff,"%5d",aacx);
        LCD_ShowString(30+40, 128-64,   strff, WHITE,BLACK,16,0);
        memset(strff,0,sizeof(strff));sprintf(strff,"%5d",aacy);
        LCD_ShowString(30+40, 144-64,   strff, WHITE,BLACK,16,0);
        memset(strff,0,sizeof(strff));sprintf(strff,"%5d",aacz);
        LCD_ShowString(30+40, 160-64,   strff, WHITE,BLACK,16,0);
        memset(strff,0,sizeof(strff));sprintf(strff,"%5d",gyrox);
        LCD_ShowString(128+40, 128-64,   strff, WHITE,BLACK,16,0);
        memset(strff,0,sizeof(strff));sprintf(strff,"%5d",gyroy);
        LCD_ShowString(128+40, 144-64,   strff, WHITE,BLACK,16,0);
        memset(strff,0,sizeof(strff));sprintf(strff,"%5d",gyroz);
        LCD_ShowString(128+40, 160-64,   strff, WHITE,BLACK,16,0);
        // tos_task_delay(200);

        unsigned long now = millis();                       //当前时间(ms)
        dt = (now - lastTime) / 1000.0;                     //微分时间(s)
        lastTime = now;                                     //上一次采样时间(ms)

        ax = (float)aacx;
        ay = (float)aacy;
        az = (float)aacy;

        gx = (float)gyrox;
        gy = (float)gyroy;
        gz = (float)gyroy;

        float accx = ax / AcceRatio;                        // x轴加速度
        float accy = ay / AcceRatio;                        // y轴加速度
        float accz = az / AcceRatio;                        // z轴加速度

        aax = atan(accy / accz) * (-180) / pi; // y轴对于z轴的夹角
        aay = atan(accx / accz) * 180 / pi;    // x轴对于z轴的夹角
        aaz = atan(accz / accy) * 180 / pi;    // z轴对于y轴的夹角

        aax_sum = 0; // 对于加速度计原始数据的滑动加权滤波算法
        aay_sum = 0;
        aaz_sum = 0;

        for (int i = 1; i < n_sample; i++)
        {
            aaxs[i - 1] = aaxs[i]; //替换算法
            aax_sum += aaxs[i] * i;
            aays[i - 1] = aays[i];
            aay_sum += aays[i] * i;
            aazs[i - 1] = aazs[i];
            aaz_sum += aazs[i] * i;
        }

        aaxs[n_sample - 1] = aax;
        aax_sum += aax * n_sample;
        aax = (aax_sum / (11 * n_sample / 2.0)) * 9 / 7.0; //角度调幅至0-90°
        aays[n_sample - 1] = aay;                          //此处应用实验法取得合适的系数
        aay_sum += aay * n_sample;                         //本例系数为9/7
        aay = (aay_sum / (11 * n_sample / 2.0)) * 9 / 7.0;
        aazs[n_sample - 1] = aaz;
        aaz_sum += aaz * n_sample;
        aaz = (aaz_sum / (11 * n_sample / 2.0)) * 9 / 7.0;

        float gyrox = -(gx - gxo) / GyroRatio * dt; // x轴角速度
        float gyroy = -(gy - gyo) / GyroRatio * dt; // y轴角速度
        float gyroz = -(gz - gzo) / GyroRatio * dt; // z轴角速度

        agx += gyrox; // x轴角速度积分
        agy += gyroy; // x轴角速度积分
        agz += gyroz;

        Sx = 0;
        Rx = 0; /* kalman start */
        Sy = 0;
        Ry = 0;
        Sz = 0;
        Rz = 0;

        for (int i = 1; i < 10; i++)
        {
            //测量值平均值运算
            a_x[i - 1] = a_x[i]; //即加速度平均值
            Sx += a_x[i];
            a_y[i - 1] = a_y[i];
            Sy += a_y[i];
            a_z[i - 1] = a_z[i];
            Sz += a_z[i];
        }
        a_x[9] = aax;
        Sx += aax;
        Sx /= 10; // x轴加速度平均值
        a_y[9] = aay;
        Sy += aay;
        Sy /= 10; // y轴加速度平均值
        a_z[9] = aaz;
        Sz += aaz;
        Sz /= 10; // Z轴加速度平均值

        for (int i = 0; i < 10; i++)
        {
            Rx += pow(a_x[i] - Sx, 2);
            Ry += pow(a_y[i] - Sy, 2);
            Rz += pow(a_z[i] - Sz, 2);
        }
        Rx = Rx / 9; //得到方差
        Ry = Ry / 9;
        Rz = Rz / 9;

        Px = Px + 0.0025;             // 0.0025在下面有说明...
        Kx = Px / (Px + Rx);          //计算卡尔曼增益
        agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加
        Px = (1 - Kx) * Px;           //更新p值

        Py = Py + 0.0025;
        Ky = Py / (Py + Ry);
        agy = agy + Ky * (aay - agy);
        Py = (1 - Ky) * Py;

        Pz = Pz + 0.0025;
        Kz = Pz / (Pz + Rz);
        agz = agz + Kz * (aaz - agz);
        Pz = (1 - Kz) * Pz; /* kalman end */

        int Accy = accy * 100;
        int Accx = accx * 100;

        printf("Accx %d, Accy %d\t", Accx, Accy);

        memset(strff,0,sizeof(strff));sprintf(strff,"%d",Accx);
        LCD_ShowString(30+50, 160+24-64,   strff, WHITE,BLACK,16,0);

        memset(strff,0,sizeof(strff));sprintf(strff,"%d",Accy);
        LCD_ShowString(128+50, 160+24-64,   strff, WHITE,BLACK,16,0);  

        tos_task_delay(100);              
	}
}

int mpu6050_task_init(void)
{
    k_err_t err;

    err = tos_task_create(&mpu6050_task, "mpu6050", mpu6050_task_entry, NULL, 3, mpu6050_stk, mpu6050_stk_size, 0);

    return err == K_ERR_NONE ? 0 : -1;
}

在上述代码中，MPU_Get_Accelerometer()、MPU_Get_Gyroscope()由mpu6050数据读取模块提供，读取出来原始数据，进行一定的才有初始化，然后正式读取时，使用卡尔曼滤波处理，得到最终的有效运动控制数据。

因为控制BB-8仅需要两个方向，所以最终得到的是Accx、Accy。

MQTT模块

根据获取的运动信息Accx、Accy，生成控制指令：{"action":"move","x":%d,"y":%d}
BB-8运动时，原点为(150, 150)，且都为正值。根据运动方向，在此基础上增减，即可控制BB-8运动。向前运动时，Y增大，反之减小；向左运动时，X减小，反之增大；【备注：MQTT还在调试进行中，今晚搞定】

使用说明

操作步骤

BB-8准备 1) 将BB-8上电，会自动点亮
控制转发系统准备 1）在电脑上运行网页控制系统，以移动设备模式查看：

并在此界面上，点击连接，在弹出界面中，选择当前激活的BB-8，以便连接到BB-8

2）在电脑上运行测试转发系统：

可以在这个界面上，发送指令，测试控制是否正常。

开发板运行
image.png

演示视频

【实际控制BB-8运动的截图，今晚录制】

总结

本次时间本来比较充分，但是没有做好时间安排，MPU6050数据读取、结算等都很快实现了，但是MQTT的部分没有想到卡了壳。

本次提供的CH32V307_EVB开发板，接口和资源非常的丰富。除了本次TOS提供的实例，也可以参考沁恒官方的实例。

TencentOS Tiny提供的实例，也非常好用。总体而言，使用TencentOS Tiny开发的难度并不大，但是有一点需要注意的是，各项组件的使用，并不是那么方便，有的组件提供了，并不一定能在新适配的板子上直接用上。这一点，有友商的系统在组件方面，就做的比较好，通过界面进行设置启用，就会自动应用，不论是什么开发板。在这方面需要提升。

原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。

如有侵权，请联系 cloudcommunity@tencent.com 删除。

tencentos tiny