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

乘简

【平头哥RVB2601创意应用开发】+ 平衡小车 [复制链接]

 

一、项目背景

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显示部分代码

#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[4][i]=g_oled_ram[4][i+1];
		g_oled_ram[5][i]=g_oled_ram[5][i+1];
		g_oled_ram[6][i]=g_oled_ram[6][i+1];
		g_oled_ram[7][i]=g_oled_ram[7][i+1];
	}
}

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

//显示3个角度数据,y为y坐标起始地址
void showAngle(int y)
{
	char str[30];
	sprintf(str,"x%3.2f  ",angleX);
	
	sprintf(&str[7],"y%3.2f  ",angleY);
	
	sprintf(&str[14],"z%3.2f  ",angleZ);
	str[21]=0;
	TextOut(0,y,str,12);
}

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

	sprintf(&str[7],"v%4.1f  ",V_Pwm);
	
	sprintf(&str[14],"t%4.1f  ",T_Pwm);
	str[21]=0;
	
	TextOut(0, y,str,12);
}

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

	sprintf(&str[7],"kd%2.2f  ",pid.bkd);
	
	sprintf(&str[14],"vp%3.1f  ",pid.vkp);
	str[21]=0;
	
	TextOut(0, y,str,12);
}

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

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

void showEsd(int y)
{
	char str[30];
	sprintf(&str[0],"e%5d    ",(int)sum_e_Velocity);
	sprintf(&str[6],"z%4d    ",(int)sum_e_AngleY);
	sprintf(&str[11],"zd%3d   ",(int)zd);
	str[21]=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();//刷新屏幕
}

2、陀螺仪代码

#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_IDX  0
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[2];
    buf[0]=REG_Address;
	buf[1]=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[2]={0,0};
	
	MPU6050_ReadReg(REG_Address);
	csi_iic_master_receive(&master_iic, SLAVE_ADDR, buf, 2, 100);
	H=buf[0];
	L=buf[1];
	
	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为+-250  0x08为+-500  0x10为+-1000  0x18为+-2000度
	MPU6050_WriteReg(ACCEL_CONFIG, 0x00);//加速度 0x00为+-2g  0x08为+-4g  0x10为+-8g  0x18为+-16g
	MPU6050_WriteReg(PWR_MGMT_1, 0x01);	//解除休眠状态
	
	aos_timer_new(&mpu_timer, timer_mpu6050, NULL, 10, 1);//10ms定时器
}

3、平衡部分代码

#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;//统计完后清零
}

五、作品源码

pdcar.zip (32.46 KB, 下载次数: 9)

2022-6-6 21:03 上传

点击文件名下载附件

 

六、视频演示

七、项目总结

  （一）陀螺仪相关

（二）wifi遥控器相关

 

八、其他

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

 

nmg

这个作品竟然做出来了，出乎我意料

乘简

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

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

 

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

 

乘简

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

 

nmg

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

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

lugl4313820

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

吾妻思萌

动手能力拉满，点赞

乘简

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

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

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

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

 

改天再装起来试试