【平头哥RVB2601创意应用开发】+ 平衡小车

乘简发表于 2022-6-6 21:06

一、项目背景

RVB2601是基于平头哥生态芯片CH2601设计的一款开发板，板载JTAG调试器、WiFi&BLE芯片W800,音频ADC-ES7210,音频DAC-ES8156,128X64 OLED屏幕，RGB三色指示灯，用户按键，兼容Arduino的扩展接口。非常适合智能家居领域的各种场景应用开发。想来未来是各种机器人的时代，正好可以学一下这款芯片，就拿平衡小车来练练手了。

二、作品简介

本平衡小车采用，两节18650电池串连供电，一个5V的降压模块，给RVB2601供电，开发板上的3.3V用来给MPU6050陀螺仪模块供电，用的IIC与陀螺仪通信，另下还有一DRV8833电机驱动模块，驱动两个带编码器的电机，所以需要4路PWM，每个电机还有2路编码器数据采集，所以一共需要8个IO口，因为开发自带了WIFI，所以遥控部分就用wifi，内嵌一个小的web服务器，这样只要手机打开浏览器即可实现遥控平衡小车的运行。

三、系统框图

 四、各部分功能说明和解析

    1、OLED显示部分代码

<pre>
<code class="language-cpp">#include "display.h"
#include "ssd1309.h"
#include "wifi_w800.h"

//显示曲线, r取值为-16~+16
void showQx(int r)
{
static int b=48;
if(r<0)
{
if(r<-16)r=-16;//-16~-1
r+=16;//0~15
r+=32;//32~47
}
else{
if(r>15)r=15;//0~15
r+=48;//48~63
}
oled_draw_point(48, 127, 1);//画中线

oled_draw_point(b, 127, 0);//清除上次的值
oled_draw_point(r, 127, 1);

b=r;

for(int i=0;i<127;i++){
g_oled_ram=g_oled_ram;
g_oled_ram=g_oled_ram;
g_oled_ram=g_oled_ram;
g_oled_ram=g_oled_ram;
}
}

void showIP(int y)
{
TextOut(0, y,"IP:",12);
TextOut(24,y,ip,12);
}

//显示3个角度数据,y为y坐标起始地址
void showAngle(int y)
{
char str;
sprintf(str,"x%3.2f",angleX);

sprintf(&str,"y%3.2f",angleY);

sprintf(&str,"z%3.2f",angleZ);
str=0;
TextOut(0,y,str,12);
}

//显示3项pwm数据
void showPwm(int y)
{
char str;
sprintf(str,"p%4.1f",B_Pwm);

sprintf(&str,"v%4.1f",V_Pwm);

sprintf(&str,"t%4.1f",T_Pwm);
str=0;

TextOut(0, y,str,12);
}

//显示平衡pid与振动次数
void showPid(int y)
{
char str;
sprintf(str,"kp%3.1f",pid.bkp);

sprintf(&str,"kd%2.2f",pid.bkd);

sprintf(&str,"vp%3.1f",pid.vkp);
str=0;

TextOut(0, y,str,12);
}

void showSd(int y)
{
char str;
sprintf(&str,"sd%-3d",Velocity);
sprintf(&str,"%3.2f",sd);
sprintf(&str,"L%4d ",(int)Pluse_L);
sprintf(&str,"R%4d ",(int)Pluse_R);
str=0;
TextOut(0, y,str,12);
}

void showOp(int x,int y)
{
if(gb==0)CharOut(x,y,24,12,CUSTOM12,12);
if(gb==1)CharOut(x,y,24,12,CUSTOM12,12);
if(gb==2)CharOut(x,y,24,12,CUSTOM12,12);
if(gb==3)CharOut(x,y,24,12,CUSTOM12,12);
if(gb==4)CharOut(x,y,24,12,CUSTOM12,12);
if(gb==5)CharOut(x,y,24,12,CUSTOM12,12);
}

void showEsd(int y)
{
char str;
sprintf(&str,"e%5d ",(int)sum_e_Velocity);
sprintf(&str,"z%4d ",(int)sum_e_AngleY);
sprintf(&str,"zd%3d ",(int)zd);
str=0;
TextOut(0, y,str,12);
}

void show()
{
static int p=0;

if(page==0){
if(p!=page){//切换页，先清屏
p=page;
oled_clear();
}
showIP(0);//显示IP
showQx(angleY);//+-90度，就是180，要显示在32高的位置上，180/32=5.625);
showAngle(12);//显示角度
}
else if(page==1){
if(p!=page){//切换页，先清屏
p=page;
oled_clear();
}

showSd(0);
showEsd(12);
showQx(sd*2);
}
else if(page==2){
if(p!=page){//切换页，先清屏
p=page;
oled_clear();
}

showAngle(0);//显示角度
showSd(12);//
showQx(Pluse_L/18.75);//显示振动曲线
}
else{
if(p!=page){//切换页，先清屏
p=page;
oled_clear();
}

showPwm(0);//显示3项pwm数据
showAngle(12);
showSd(24);
showEsd(36);
showOp(52,48);
}

oled_reflesh();//刷新屏幕
}
</code></pre>

2、陀螺仪代码

<pre>
<code class="language-cpp">#include "mpu6050.h"
#include <drv/iic.h>
#include <aos/aos.h>
#include <drv/pin.h>
#include <math.h>
#include "pidcar.h"
#include "wifi_w800.h"

#define IIC_IDX0
static csi_iic_t master_iic;
static aos_timer_t mpu_timer;//定时器

int16_t rawAccX=0,rawAccY=0,rawAccZ=0,rawGyroX=0,rawGyroY=0,rawGyroZ=0;//6项原始数据

float angleX=0, angleY=0, angleZ=0;
float angleAccX=0,angleAccY=0; //由mpu6050_tockn加计算的加速度角度
float gyroX, gyroY, gyroZ;

//float gyroXoffset=0,gyroYoffset=0,gyroZoffset=0;

int iic_init(void)
{
csi_error_t ret;

csi_pin_set_mux(PA8, PA8_IIC0_SCL);
csi_pin_set_mux(PA9, PA9_IIC0_SDA);

ret = csi_iic_init(&master_iic, IIC_IDX);
if (ret != CSI_OK) {
printf("csi_iic_initialize error\n");
return -1;
}
/* config iic master mode */
ret = csi_iic_mode(&master_iic, IIC_MODE_MASTER);
if (ret != CSI_OK) {
 printf("csi_iic_set_mode error\n");
 return -1;
}
/* config iic 7bit address mode */
ret = csi_iic_addr_mode(&master_iic, IIC_ADDRESS_7BIT);
if (ret != CSI_OK) {
 printf("csi_iic_set_addr_mode error\n");
 return -1;
}
/* config iic standard/fast speed*/
ret = csi_iic_speed(&master_iic, IIC_BUS_SPEED_FAST);//IIC_BUS_SPEED_STANDARD);
if (ret != CSI_OK) {
 printf("csi_iic_set_speed error\n");
 return -1;
}
return 0;
}

//写寄存器
void MPU6050_WriteReg(uint8_t REG_Address, uint8_t REG_data)
{
uint8_t buf;
buf=REG_Address;
buf=REG_data;
csi_iic_master_send(&master_iic, SLAVE_ADDR, buf, 2, 100);
}

//读寄存器
void MPU6050_ReadReg(uint8_t REG_Address)
{
uint8_t buf;
buf=REG_Address;
csi_iic_master_send(&master_iic, SLAVE_ADDR, &buf, 1, 100);
}

//读数据
int16_t MPU6050_ReadData(uint8_t REG_Address)
{
uint8_t H,L;
uint8_t buf={0,0};

MPU6050_ReadReg(REG_Address);
csi_iic_master_receive(&master_iic, SLAVE_ADDR, buf, 2, 100);
H=buf;
L=buf;

return (H<<8)|L;
}

void MPU6050_Get_Data()
{
float accX, accY, accZ;
// float homeAccX,homeAccY;

//6项原始数据
rawAccX = MPU6050_ReadData(ACCEL_XOUT_H);
rawAccY = MPU6050_ReadData(ACCEL_YOUT_H);
rawAccZ = MPU6050_ReadData(ACCEL_ZOUT_H);
rawGyroX = MPU6050_ReadData(GYRO_XOUT_H);
rawGyroY = MPU6050_ReadData(GYRO_YOUT_H);
rawGyroZ = MPU6050_ReadData(GYRO_ZOUT_H);

//计算重力加速度的g数
accX = ((float)rawAccX) / 16384.0;//+-2g，65536/4=16384
accY = ((float)rawAccY) / 16384.0;
accZ = ((float)rawAccZ) / 16384.0;

//角速度
gyroX = ((float)rawGyroX) / 65.536;//+-500
gyroY = ((float)rawGyroY) / 65.536;
gyroZ = ((float)rawGyroZ) / 65.536;

//由重力加速度计算X、Y轴的角度，tockn库
angleAccX = atan2(accY, accZ + abs(accX)) * RAD_TO_DEG;//360 / 2.0 / PI;
angleAccY = -atan2(accX, accZ + abs(accY)) * RAD_TO_DEG;
//angleAccY += MPU_OFFSET_Y;

angleAccY += 1.15;//此参数为陀镙仪固定在小车上的角度物理误差，根据经验调整

// gyroX -= gyroXoffset;
// gyroY -= gyroYoffset;
// gyroZ -= gyroZoffset;

// //平衡小车之家的融合算法
// homeAccX = 0;
// homeAccY = -atan2((float)rawAccX, (float)rawAccZ) * RAD_TO_DEG;
// homeAccY+=MPU_OFFSET_Y;
// homeX = 0.02 * homeAccX + 0.98 * (homeX + gyroX * 0.01);
// homeY = 0.02 * homeAccY + 0.98 * (homeY + gyroY * 0.01);

//tockn库角度融合算法，上次的角度+10ms的角速度占98%，本次的角度占2%
angleX = (0.98f * (angleX + gyroX * 0.01)) + (0.02f * angleAccX);
angleY = (0.98f * (angleY + gyroY * 0.01)) + (0.02f * angleAccY);
angleZ = gyroZ*0.01;

}

//定时器回调函数
static void timer_mpu6050(void *arg1, void* arg2)
{

MPU6050_Get_Data();

//pid_car();

static int t=0;
t++;
t%=10;
if(t==0){
printf("%12f,%12f,%12f\r\n",angleX,angleY,angleZ);
}
}

void mpu6050_init()
{
iic_init();//初始化iic总线
MPU6050_WriteReg(SMPLRT_DIV, 0x00);//陀螺仪125hz
MPU6050_WriteReg(CONFIG, 0x00); //21HZ滤波延时A8.5ms G8.3ms此处取值应相当注意，延时与系统周期相近为宜
MPU6050_WriteReg(GYRO_CONFIG, 0x08); //陀螺仪 0x00为+-2500x08为+-5000x10为+-10000x18为+-2000度
MPU6050_WriteReg(ACCEL_CONFIG, 0x00);//加速度 0x00为+-2g0x08为+-4g0x10为+-8g0x18为+-16g
MPU6050_WriteReg(PWR_MGMT_1, 0x01); //解除休眠状态

aos_timer_new(&mpu_timer, timer_mpu6050, NULL, 10, 1);//10ms定时器
}
</code></pre>

3、平衡部分代码

<pre>
<code class="language-cpp">#include "pidcar.h"
#include "pwm_mada.h"
#include "mpu6050.h"
#include "wifi_w800.h"
#include "counter.h"
#include <math.h>

#define constrain(p) {if(p<-pwm_width)p=-pwm_width;else if(p>pwm_width)p=pwm_width;}

int Velocity=0;
float sd=0;
int zd=0;//振断次数
float sum_e_Velocity=0, sum_e_AngleY=0;
float Angle_Turn_Setting=0, V_Setting;
float B_Pwm, V_Pwm, T_Pwm, Pluse_L, Pluse_R;
//bkp为100时，高频振动，bkd为2.8时高频振动，所以*0.6=60和1.68
//再调vkp，调到60的时候，发现基本能定在一个位置了
PID pid={60, 1.6, 60, 25, 1};
float Blance_JD=0;//平衡时的角度

//平衡PD
void Blance_PD() //blance pid
{
//float Kp = 18;//在关Kd时，调36会前后摇摆往一边驶去，18会越来越快往一边驶去，直到倒掉
//float Kd = 0.3;//1.4时，高频抖.1.4*0.6=0.8
float p = pid.bkp * angleY + pid.bkd * gyroY;

B_Pwm = B_Pwm * 0.5 + p * 0.5;

constrain(B_Pwm);//限制B_Pwm值超过pwm_width
}

//检测每秒振动次数
void CheckZd()
{
static float angle_Last=0;
static int i=0;
if(angle_Last>0 && angleY<0 || angle_Last<0 && angleY>0){//与上次
zd++;
}
if(++i>=100){//每秒钟清零
i=zd=0;
}
angle_Last=angleY;
}

//速度
void Velocity_PI()//velocity pid
{
float e_Velocity = Velocity - V_Setting;
float p;

sd=0.7 * sd + 0.3 * e_Velocity;

sum_e_Velocity += sd;
constrain(sum_e_Velocity);

p = pid.vkp * sd + pid.vkp * sum_e_Velocity / 200;
V_Pwm = V_Pwm*0.5 + p*0.5;

constrain(V_Pwm);
//V_Pwm=0;
}

//旋转
void Turn_PID_Angle()//angle of Z axis pid
{
float e_AngleZ = (Count_Left - Count_Right) - Angle_Turn_Setting;

sum_e_AngleY += e_AngleZ;

T_Pwm = pid.tkp * e_AngleZ + pid.tki * sum_e_AngleY;

//T_Pwm=0;
}

void reset()
{
gb=0;//停止运行，要再次按键
sum_e_Velocity = 0;
sum_e_AngleY = 0;
B_Pwm = 0;
V_Pwm = 0;
T_Pwm = 0;
sd=0;
sd_zq=sd_zh=sd_yq=sd_yh=0;
}

void pid_car()
{
float l,r;

Velocity = (Count_Left + Count_Right);//记录10ms的电机转动速度

if (gb==2){//前进
V_Setting = 2;
}
else if(gb==4){//后退
V_Setting = -2;
}
else{//停住
V_Setting = 0;
}

if(gb==1){//左转
Angle_Turn_Setting = -2;
}
else if(gb==3){
Angle_Turn_Setting = 2;
}
else Angle_Turn_Setting = 0;

Blance_PD();
CheckZd();
Velocity_PI();
Turn_PID_Angle();

l = B_Pwm + V_Pwm - T_Pwm;
Pluse_L = Pluse_L*0.5 + l*0.5;

r = B_Pwm + V_Pwm + T_Pwm;
Pluse_R = Pluse_R*0.5 + r*0.5;

constrain(Pluse_L);
constrain(Pluse_R);

if (gb && angleY>-30 && angleY < 30) //if angleY is between -65 and 65 ,run it
{

pwm_start();
if (Pluse_L > 0)
{
sd_zq=abs((int)Pluse_L);
pwm_zq();
}
else
{
sd_zh=abs((int)Pluse_L);
pwm_zh();
}

if (Pluse_R > 0)
{
sd_yq=abs((int)Pluse_R);
pwm_yq();
}
else
{
sd_yh=abs((int)Pluse_R);
pwm_yh();
}
}
else
{
reset();
pwm_stop();
}

Count_Left = Count_Right = 0;//统计完后清零
}
</code></pre>

五、作品源码



 

六、视频演示

<iframe allowfullscreen="true" frameborder="0" height="450" src="//player.bilibili.com/player.html?bvid=1pY411g7RM&page=1" style="background:#eee;margin-bottom:10px;" width="700"></iframe> 
七、项目总结

  <a href="https://bbs.eeworld.com.cn/thread-1205777-1-1.html" target="_blank">（一）陀螺仪相关</a>

<a href="https://bbs.eeworld.com.cn/thread-1205776-1-1.html" target="_blank">（二）wifi遥控器相关</a>

 

八、其他

第一次用这个开发板，各种不顺手，主要原因是，平头哥的开发环境，必须要内嵌一个OS在里面的，这和我之前学的51大有不同，且IDE也不太友好，各种BUG多，还有驱动中也存在一些BUG，好在有平头哥小助手及工单系统的帮助，总算完成了，期间多次想放弃，退回开发板不想弄了，但最终还是坚持下来了，希望平头哥解决掉所有BUG，使开发者用着舒心，我会持续关注，并学习我还未使用到的功能。。。

nmg 发表于 2022-6-7 08:42

<img height="50" src="https://bbs.eeworld.com.cn/static/editor/plugins/hkemoji/sticker/facebook/wanwan21.gif" width="63" />这个作品竟然做出来了，出乎我意料

乘简发表于 2022-6-7 09:23

nmg 发表于 2022-6-7 08:42
这个作品竟然做出来了，出乎我意料

其实平衡小车，大部分时间都用在调参数上了，不过我调的参数，还不是很理想，小车运行起来不是很平稳，其实真正的代码是很少。。。

 

不过交完作业才发现display.c中少#define了几个.h文件，因为交作业之前那个里面的函数是在main.c中的，后来才移到display.c中，所以就少包函了几个.h文件，但问题不大。。。

乘简发表于 2022-6-7 09:31

还有，交完作业后，我把小车拆了，可是发现开发板跑不起来了，后来发现，我用的IIC函数不对，我用的是同步函数，里面有100的超时处理，当没有接上陀螺仪的时候，是会卡住的，应该改用异步的IIC库函数，应该就没有问题了。。。

nmg 发表于 2022-6-7 09:34

乘简发表于 2022-6-7 09:31
还有，交完作业后，我把小车拆了，可是发现开发板跑不起来了，后来发现，我用的IIC函数不对，我用的是同步 ...

先别着急拆作品啊，如果平头哥那边想二次包装，估计需要你邮寄作品

lugl4313820 发表于 2022-6-7 15:31

楼主，是最优秀的哥，声音也好有磁性哦，加油，期待更精彩的作品。

吾妻思萌 发表于 2022-6-8 08:34

动手能力拉满，点赞

乘简发表于 2022-6-8 08:44

nmg 发表于 2022-6-7 09:34
先别着急拆作品啊，如果平头哥那边想二次包装，估计需要你邮寄作品

我好像又发现代码中的一个小BUG，就是

Count_Left = Count_Right = 0;//统计完后清零

这条语句写得太远了，，应该获取到值后立即清零，或者干脆不清零，用别的方法统计，，要不可能会丢失一些编码器数据

 

改天再装起来试试

页: [1]

电子工程世界-论坛's Archiver

【平头哥RVB2601创意应用开发】+ 平衡小车