【瑞萨RA MCU创意氛围赛】瑞萨RA蓝牙平衡小车

克哈keha · 发表于 2023-7-5 17:51:20

本帖最后由克哈keha 于 2023-7-5 21:39 编辑
一、简介
1.平衡小车运行模式（通过蓝牙控制）：（1）平衡模式（2）跟随模式（3）避障模式。
2.手机控制小车前进后退转向加减速等。
3.小车拿起、放下检测（控制小车是否运行）
4.可通过Type-C数据线与上位机调试
5.预留NRF24L01、红外接口、多路电源接口可以拓展开发
6.后续待开发......

二、展示
平衡小车视频（后续更新全视频）：

平衡小车运行模式.zip (8.17 MB, 下载次数: 149)

小车实物图

RA2小板实物图

RA2小板

RA平衡小车底板

三、简介

瑞萨RA平衡小车系统框架如下图：

2串电池产生7.2V，输入到电源模块产生5V，再通过LDO产生3.3V。

平衡模式：通过MPU6050检测小车姿态，RA MCU控制电机驱动模块控制电机运行，并显示到OLED上。

跟随模式：小车通过超声波模块检测距离，在一定距离内开启跟踪。

避障模式：小车直行，通过超声波模块判断前方是否存在障碍物，若遇到障碍物将转弯。

蓝牙：手机通过与蓝牙模块链接，控制小车运行模式，并可对小车进行控制。

四、模块准备

由于开发板不易于与底板链接，所以自行制作了一块小板:

RA2小板原理图

平衡小车底板板载串口模块与LDO:

平衡小车底板串口模块

电源模块：LM2596S DC-DC降压电源模块

MPU6050模块：MPU-6050模块三轴加速度+三轴陀螺仪

OLED模块：0.96寸 OLED显示液晶屏模块（注意区分VCC与GND引脚顺序，下图不正确）

电机驱动模块：TB6612FNG电机驱动模块

蓝牙模块：蓝牙3.0模块 SPP透传兼容HC-05/06从机 JDY-31

暂时先更新这些，后续更新......

克哈keha · 发表于 2023-8-21 23:45:40

放一边太久差点忘记了。。。

B站视频：https://www.bilibili.com/video/BV1x84y1Z7HQ/?spm_id_from=333.999.0.0&vd_source=e9a41e618602e99bc20cf9fbfd79c4c9

五、部分内容介绍

通过蓝牙控制平衡小车模式，平衡模式下小车前进后退左转右转，加速减速停止，开机上电默认平衡模式。（1平衡模式 2跟踪模式 3避障模式）

平衡小车倒下检测：当小车倒下（到达一定角度），停止电机

平衡小车拿起检测：当小车被拿起来（轮子高速转动一定时间），停止电机

平衡小车放下检测：当小车重新被放下一定时间（轮子没有转动并且角度小于一定范围），重新进入平衡模式

PID控制:

<div style="background-color: rgb(255, 255, 255); padding-left: 2px;">
<div style="color: rgb(0, 0, 0); font-family: Consolas; font-size: 14pt; text-wrap: nowrap;">
_PID_CONTROL vel = {130.0f, 0.65f, 0.0f};_PID_CONTROL bal = {300.0f, 0.0f , 20.0f};_PID_CONTROL tur = {5.0f, 0.0f, 10.0f};
/*</div></div>

复制代码

输出控制：

/*
* 描述：控制两路PWM输出
*/
uint32_t pwm_1=0,duty_1=0;
uint32_t pwm_2=0,duty_2=0;
void motor_pwm_out(uint16_t pwm1,uint16_t pwm2)
{
pwm_1 = to_limit(pwm1,0,7199);
pwm_2 = to_limit(pwm2,0,7199);
R_GPT_DutyCycleSet(&g_pwm1_ctrl, pwm_1, GPT_IO_PIN_GTIOCA);
R_GPT_DutyCycleSet(&g_pwm2_ctrl, pwm_2, GPT_IO_PIN_GTIOCA);
}
/*
* 描述：控制输出
*/
void _ctr_out(void)
{
uint8_t i;
if(PickUpTheCar(att.rol,hall1count,hall2count) == 1) //小车被拿起检测
{
theCarStopFlag = 1; //停止运行标志
}
if(PutDownTheCar(att.rol,hall1count,hall2count)) //小车被放下检测
{
theCarStopFlag = 0; //运行标志
}
/* 检测是否停止（倒下或被拿起），倒下或者停止后，积分项清零(不清除会干扰下次平衡)，并且电机停转 */
if( shut_down(att.rol,70.0f) || theCarStopFlag == 1)
{
//#endif
PickUpisOK = 0;
for( i = 0 ; i < 3 ; i++) //清除所有偏差
{
ctr.out[i] = 0;
ctr.bais[i] = 0;
}
ctr.motor[0] = 0;
ctr.motor[1] = 0;
}
else
{
ctr.motor[0] = ctr.out[0];
ctr.motor[1] = ctr.out[1];
}
if( theCarStopFlag == 0)
motor_pwm_out(ctr.motor[0],ctr.motor[1]); //正常输出
else
motor_pwm_out(0,0); //关闭电机
}
/*
* 描述：电机转向控制
* 输入：motor1电机1的pwm，motor2电机2的pwm
*/
void dir_config(int *motor1,int *motor2)
{
if(*motor1<0) AIN1_HIGH, AIN2_LOW;
else AIN1_LOW, AIN2_HIGH;
*motor1 = int_abs(*motor1);
if(*motor2<0) BIN1_LOW, BIN2_HIGH;
else BIN1_HIGH, BIN2_LOW;
*motor2 = int_abs(*motor2);
}

复制代码

hal_entry.c:

#include "hal_data.h"
#include "controller.h"
#include "oled.h"
#include "i2c_soft.h"
#include "mpu6050.h"
#include "imath.h"
#include "imu.h"
FSP_CPP_HEADER
void R_BSP_WarmStart(bsp_warm_start_event_t event);
FSP_CPP_FOOTER
bsp_io_level_t p_port_value_port_104; //ECHO引脚
uint32_t Hightime_counter = 0; //高电平时间
float dis =0; //距离
uint8_t dis_Start=0; //测量开始标志
uint8_t Dis=0; //测量距离cm
bsp_io_level_t p_port_value_port_212;//马达1 （B相）通道1
bsp_io_level_t p_port_value_port_213;//马达1 （A相）通道2
bsp_io_level_t p_port_value_port_408;//马达2 （A相）通道7
bsp_io_level_t p_port_value_port_409;//马达2 （B相）通道6
bsp_io_level_t p_port_value_port_015; //ECHO引脚
uint8_t direction_a=0;
uint8_t direction_b=0;
int hall1count_a=0;
int hall1count_b=0;
int hall2count_a=0;
int hall2count_b=0;
int hall1count=0;
int hall2count=0;
volatile bool uart_send_complete_flag = false;
#define USART_REC_LEN 200 //定义最大接收字节数 200
char USART_RX_BUF[USART_REC_LEN]; //接收缓冲,最大USART_REC_LEN个字节.末字节为换行符
uint16_t USART_RX_STA=0; //接收状态标记
uint8_t mode=0; // 0平衡模式 1跟踪模式 2避障模式
uint8_t move=0; // 0停止 1前 2后 3左 4右（方向控制仅在平衡模式下）
uint8_t speed=0; // 1加5 2减5
uint16_t RxLine=0; //接收到的数据长度
#ifdef __GNUC__ //串口重定向
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE
{
R_SCI_UART_Write(&g_uart9_ctrl, (uint8_t *)&ch, 1);
while(uart_send_complete_flag == false){}
uart_send_complete_flag = false;
return ch;
}
int _write(int fd,char *pBuffer,int size)
{
for(int i=0;i<size;i++)
{
__io_putchar(*pBuffer++);
}
return size;
}
void user_uart9_callback(uart_callback_args_t * p_args)
{
if(p_args->event == UART_EVENT_TX_COMPLETE)
{
uart_send_complete_flag = true;
}
}
void user_uart_callback(uart_callback_args_t * p_args)
{
uint8_t RxBuff;
if(p_args->event == UART_EVENT_TX_COMPLETE)
{
uart_send_complete_flag = true;
}
if(p_args->event == UART_EVENT_RX_CHAR)
{
RxBuff = p_args->data;
if((USART_RX_STA&0x8000)==0)//接收未完成
{
if(USART_RX_STA&0x4000)//接收到了0x0d
{
if(RxBuff!=0x0a)
{
USART_RX_STA=0;//接收错误,重新开始
memset(USART_RX_BUF,0,sizeof(USART_RX_BUF)); //清空缓存数组
}
else USART_RX_STA|=0x8000; //接收完成了
}
else //还没收到0X0D，继续接收来的数据
{
if(RxBuff==0x0d)USART_RX_STA|=0x4000;//接收到了0x0d
else//接收数据
{
USART_RX_BUF[USART_RX_STA&0X3FFF]=RxBuff ;
USART_RX_STA++;
if(USART_RX_STA>(USART_REC_LEN-1))
{
USART_RX_STA=0;//接收数据长度超过最大接收还没收到0x0d,接收数据错误,重新开始接收
memset(USART_RX_BUF,0,sizeof(USART_RX_BUF)); //清空缓存数组
}
}
}
}
}
}
void ParseBluetoothMessage(char *pInput)
{
if(strcmp(pInput,"mode=0")==0)mode=0;//平衡模式
if(strcmp(pInput,"mode=1")==0)mode=1;//跟踪模式
if(strcmp(pInput,"mode=2")==0)mode=2;//避障模式
if(strcmp(pInput,"move=0")==0)move=0;//停止
if(strcmp(pInput,"move=1")==0)move=1;//前进
if(strcmp(pInput,"move=2")==0)move=2;//后退
if(strcmp(pInput,"move=3")==0)move=3;//左转
if(strcmp(pInput,"move=3")==0)move=4;//右转
if(strcmp(pInput,"speed=1")==0)speed=1;//加5
if(strcmp(pInput,"speed=2")==0)speed=2;//减5
}
void SR04_OLED(void)
{
/* 超声波测量信号 */
dis_Start=1;
R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_10, 1);
R_BSP_SoftwareDelay (20, BSP_DELAY_UNITS_MICROSECONDS);
R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_10, 0);
/* OLED显示 */
OLED_ShowString(0,0,"ValLeft :",12);
if(hall1count>=0)
{
OLED_ShowString(82,0,"+",12);
OLED_ShowNum(90,0,hall1count,3,12);
}
else
{
OLED_ShowString(82,0,"-",12);
OLED_ShowNum(90,0,-(hall1count),3,12);
}
OLED_ShowString(0,1,"ValRight:",12);
if(hall2count>=0)
{
OLED_ShowString(82,1,"+",12);
OLED_ShowNum(90,1,hall2count,3,12);
}
else
{
OLED_ShowString(82,1,"-",12);
OLED_ShowNum(90,1,-(hall2count),3,12);
}
Dis=dis;
OLED_ShowString(0,6,"dis:",12);
OLED_ShowNum(90,6,Dis,3,12);
OLED_ShowString(0,4,"Mode:",12);
OLED_ShowNum(90,4,mode,3,12);
}
void timer3_callback(timer_callback_args_t * p_args)
{
}
void SR04_callback(external_irq_callback_args_t *p_args)
{
timer_status_t status;//获取当前计数值
if (1 == p_args->channel)
{
/*读取端口电平状态，如果是低电平则发生的是下降沿，高电平则是上升沿*/
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_01_PIN_04, &p_port_value_port_104);
if(dis_Start == 1)
{
if(p_port_value_port_104 == BSP_IO_LEVEL_HIGH)//上升沿
{
R_GPT_CounterSet(&g_timer3_ctrl, 0);
R_GPT_Start(&g_timer3_ctrl);
}
else
{
dis_Start=0;
R_GPT_StatusGet(&g_timer3_ctrl, &status);
Hightime_counter =status.counter;
dis = (float)Hightime_counter*17000/48000000 ;//(高电平时间)*340*100/2 cm 高电平时间；高电平计数次数/PCLK(48M)
R_GPT_Stop(&g_timer3_ctrl);
}
}
}
}
void hall_callback(external_irq_callback_args_t *p_args)
{
//马达1 A相上升沿检测到B相高电平为正相
/* 判断中断通道 */
if (2 == p_args->channel)//马达1 （A相）
{
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_02_PIN_13, &p_port_value_port_213);
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_02_PIN_12, &p_port_value_port_212);
if(p_port_value_port_213)//A相上升沿
{
if(p_port_value_port_212)
{
hall1count_a++;
}
else
{
hall1count_a--;
}
}
else//A相下降沿
{
if(p_port_value_port_212)
{
hall1count_a--;
}
else
{
hall1count_a++;
}
}
}
else if (3 == p_args->channel)//马达1 （B相）
{
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_02_PIN_13, &p_port_value_port_213);
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_02_PIN_12, &p_port_value_port_212);
if(p_port_value_port_212)//B相上升沿
{
if(p_port_value_port_213)
{
hall1count_a--;
}
else
{
hall1count_a++;
}
}
else//B相下降沿
{
if(p_port_value_port_213)
{
hall1count_a++;
}
else
{
hall1count_a--;
}
}
}
else if (7 == p_args->channel)//马达2 （A相）
{
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_04_PIN_08, &p_port_value_port_408);
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_04_PIN_09, &p_port_value_port_409);
if(p_port_value_port_408)//A相上升沿
{
if(p_port_value_port_409)
{
hall2count_a++;
}
else
{
hall2count_a--;
}
}
else//A相下降沿
{
if(p_port_value_port_409)
{
hall2count_a--;
}
else
{
hall2count_a++;
}
}
}
else if (6 == p_args->channel)//马达2 （B相）
{
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_04_PIN_08, &p_port_value_port_408);
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_04_PIN_09, &p_port_value_port_409);
if(p_port_value_port_409)//B相上升沿
{
if(p_port_value_port_408)
{
hall2count_a--;
}
else
{
hall2count_a++;
}
}
else//B相下降沿
{
if(p_port_value_port_408)
{
hall2count_a++;
}
else
{
hall2count_a--;
}
}
}
}
void timer0_callback(timer_callback_args_t * p_args)
{
/* 定时器更新事件 */
if (TIMER_EVENT_CYCLE_END == p_args->event)
{
/********************************* MPU6050 ***********************************/
get_gyro_raw(); //陀螺仪数据
get_deg_s(&gyro_raw_f,&Mpu.deg_s); //陀螺仪原始数据转为度为单位的速率
get_rad_s(&gyro_raw_f,&Mpu.rad_s); //陀螺仪原始数据转为弧度为单位的速率
get_acc_raw(); //加速度数据
acc_iir_lpf(&acc_raw_f,&acc_att_lpf,Mpu.att_acc_factor); //姿态解算时加速度低通滤波
get_acc_g(&acc_att_lpf,&Mpu.acc_g);
//姿态解算
mahony_update(Mpu.rad_s.x,Mpu.rad_s.y,Mpu.rad_s.z,Mpu.acc_g.x,Mpu.acc_g.y,Mpu.acc_g.z);
Matrix_ready(); //姿态解算相关矩阵更新
/********************************************************************************************************/
/* 马达霍尔计数 */
hall1count=hall1count_a+hall1count_b;
hall2count=hall2count_a+hall2count_b;
hall1count_a=0;
hall1count_b=0;
hall2count_a=0;
hall2count_b=0;
/********************************************************************************************************/
/* 小车模式判断 */
//(此行不能删掉,暂时不知道为什么)难倒要加一些延时？
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_15, &p_port_value_port_015);
// if(0==p_port_value_port_015)mode++;
// if(mode==3)mode=0;
ctr.pwm[0] = ctr_bal(att.rol,Mpu.deg_s.y); //角度直立平衡控制器
ctr.pwm[1] = ctr_vel(-hall1count,hall2count); //速度控制器
ctr.pwm[2] = ctr_turn(2400,Mpu.deg_s.z); //转向控制器
ctr.out[0] = ctr.pwm[0] + ctr.pwm[1] + ctr.pwm[2]; //电机1匹配输出
ctr.out[1] = ctr.pwm[0] + ctr.pwm[1] - ctr.pwm[2]; //电机2匹配输出
i_limit(&(ctr.out[0]),7199); //输出限幅
i_limit(&(ctr.out[1]),7199); //输出限幅
dir_config(&(ctr.out[0]),&(ctr.out[1])); //根据正负设置方向
_ctr_out();
}
}
/*******************************************************************************************************************//**
* main() is generated by the RA Configuration editor and is used to generate threads if an RTOS is used. This function
* is called by main() when no RTOS is used.
**********************************************************************************************************************/
void hal_entry(void)
{
/* TODO: add your own code here */
/* 蓝牙、串口初始化 */
R_SCI_UART_Open(&g_uart0_ctrl, &g_uart0_cfg);
//R_SCI_UART_Open(&g_uart9_ctrl, &g_uart9_cfg);
/* 马达PWM初始化 */
R_GPT_Open(&g_pwm1_ctrl, &g_pwm1_cfg);
R_GPT_Open(&g_pwm2_ctrl, &g_pwm2_cfg);
R_GPT_Start(&g_pwm1_ctrl);
R_GPT_Start(&g_pwm2_ctrl);
STBY_HIGH;
/* 编码器初始化 */
R_ICU_ExternalIrqOpen(&g_external_irq0_ctrl, &g_external_irq0_cfg);//编码器A初始化
R_ICU_ExternalIrqOpen(&g_external_irq1_ctrl, &g_external_irq1_cfg);
R_ICU_ExternalIrqOpen(&g_external_irq2_ctrl, &g_external_irq2_cfg);//编码器B初始化
R_ICU_ExternalIrqOpen(&g_external_irq3_ctrl, &g_external_irq3_cfg);
R_ICU_ExternalIrqEnable(&g_external_irq0_ctrl); /* 允许中断 */
R_ICU_ExternalIrqEnable(&g_external_irq1_ctrl);
R_ICU_ExternalIrqEnable(&g_external_irq2_ctrl);
R_ICU_ExternalIrqEnable(&g_external_irq3_ctrl);
R_GPT_Open(&g_timer0_ctrl, &g_timer0_cfg);//定时器中断 9ms
R_GPT_Start(&g_timer0_ctrl);
/* 超声波初始化 */
R_GPT_Open(&g_timer3_ctrl, &g_timer3_cfg);
R_ICU_ExternalIrqOpen(&g_external_irq4_ctrl, &g_external_irq4_cfg);
R_ICU_ExternalIrqEnable(&g_external_irq4_ctrl);
OLED_Init();
mpu6050_init();
/* 检测mpu6050是否插上 */
while(1)
{
uint8_t mpuId;
mpuId = get_mpu_id();
OLED_ShowString(10,2,"mpuId:",12);
OLED_ShowNum(90,2,mpuId,4,12);
// printf("MPU ID :%d\r\n", mpuId);
if(mpuId==0x68||mpuId==0x98) //判断mpu6050的ID是否正确
break;
R_BSP_SoftwareDelay(50, BSP_DELAY_UNITS_MILLISECONDS);
}
get_iir_factor(&Mpu.att_acc_factor,0.005f,25); //角度直立平衡控制器
while(1)
{
if(USART_RX_STA&0x8000) //接收结束标志位，这个数据可以自定义，根据实际需求，这里只做示例使用，不一定是0xff
{
RxLine=USART_RX_STA&0x3fff;
ParseBluetoothMessage(USART_RX_BUF);
// printf("length=%d\r\n",RxLine);
// for(int i=0;i<RxLine;i++)printf("data:[%d] = 0x%x\r\n",i,USART_RX_BUF[i]);
// printf("\r\n\r\n");//插入换行
USART_RX_STA=0;
memset(USART_RX_BUF,0,sizeof(USART_RX_BUF)); //清空缓存数组 ##必须加这条，否则strcmp出错##
}
SR04_OLED();
R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_12, BSP_IO_LEVEL_LOW);
R_BSP_SoftwareDelay(75, BSP_DELAY_UNITS_MILLISECONDS);
SR04_OLED();
R_BSP_SoftwareDelay(75, BSP_DELAY_UNITS_MILLISECONDS);
SR04_OLED();
R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_12, BSP_IO_LEVEL_HIGH);
R_BSP_SoftwareDelay(75, BSP_DELAY_UNITS_MILLISECONDS);
SR04_OLED();
R_BSP_SoftwareDelay(75, BSP_DELAY_UNITS_MILLISECONDS);
}
#if BSP_TZ_SECURE_BUILD
/* Enter non-secure code */
R_BSP_NonSecureEnter();
#endif
}
/*******************************************************************************************************************//**
* This function is called at various points during the startup process. This implementation uses the event that is
* called right before main() to set up the pins.
*
* @param[in] event Where at in the start up process the code is currently at
**********************************************************************************************************************/
void R_BSP_WarmStart(bsp_warm_start_event_t event)
{
if (BSP_WARM_START_RESET == event)
{
#if BSP_FEATURE_FLASH_LP_VERSION != 0
/* Enable reading from data flash. */
R_FACI_LP->DFLCTL = 1U;
/* Would normally have to wait tDSTOP(6us) for data flash recovery. Placing the enable here, before clock and
* C runtime initialization, should negate the need for a delay since the initialization will typically take more than 6us. */
#endif
}
if (BSP_WARM_START_POST_C == event)
{
/* C runtime environment and system clocks are setup. */
/* Configure pins. */
R_IOPORT_Open (&g_ioport_ctrl, g_ioport.p_cfg);
}
}
#if BSP_TZ_SECURE_BUILD
FSP_CPP_HEADER
BSP_CMSE_NONSECURE_ENTRY void template_nonsecure_callable ();
/* Trustzone Secure Projects require at least one nonsecure callable function in order to build (Remove this if it is not required to build). */
BSP_CMSE_NONSECURE_ENTRY void template_nonsecure_callable ()
{
}
FSP_CPP_FOOTER
#endif

复制代码

mpu6050.c:

#include "mpu6050.h"
#include "i2c_soft.h"
#include "filter.h"
#include <string.h>
#include "imath.h"
S16_XYZ acc_raw = {0}; //加速度计原始数据存储
S16_XYZ gyro_raw = {0}; //陀螺仪原始数据存储
SI_F_XYZ gyro_raw_cal = {0}; //陀螺仪用于校准的原始数据存储
SI_F_XYZ acc_raw_f = {0};
SI_F_XYZ gyro_raw_f = {0};
SI_F_XYZ acc_att_lpf = {0};
SI_F_XYZ gyro_lpf = {0};
SI_F_XYZ gyro_offset = {0,0,0} ; //陀螺仪零偏数据存储
_Mpu6050_data Mpu = {0};
_GYRO_CAL CalGyro = {0}; //陀螺仪校准相关变量存储
/*
* 函数名：mpu6050_init
* 描述：初始化MOU6050配置
*/
void mpu6050_init(void)
{
IIC_Write_One_Byte(0xD0,PWR_MGMT_1, 0x00); //唤醒mpu
tdelay_ms(100);
IIC_Write_One_Byte(0xD0,PWR_MGMT_2, 0x80);
/* when DLPF is disabled( DLPF_CFG=0 or 7),陀螺仪输出频率 = 8kHz;
when DLPFis enabled,陀螺仪输出频率 = 1KHz
fs(采样频率) = 陀螺仪输出频率 / (1 + SMPLRT_DIV)*/
IIC_Write_One_Byte(0xD0,SMPLRT_DIV, 0x00); //sample rate. Fsample= 1Khz/(<this value>+1) = 1000Hz
IIC_Write_One_Byte(0xD0,MPU_CONFIG, 0x03); //内部低通 acc:44hz gyro:42hz
IIC_Write_One_Byte(0xD0,GYRO_CONFIG, 0x18); // gyro scale ：+-2000°/s
IIC_Write_One_Byte(0xD0,ACCEL_CONFIG, 0x10); // Accel scale ：+-8g (65536/16=4096 LSB/g)
}
/*
* 描述：两字节数据读取并合成一个16位数据
* 输入：REG_Address寄存器地址
* 返回：合成的16位数据
*/
static int get_data(unsigned char REG_Address)
{
unsigned char H,L;
H = IIC_Read_One_Byte(0xD0,REG_Address);
L = IIC_Read_One_Byte(0xD0,REG_Address+1);
return ((H<<8)+L);
}
/*
* 描述：读取mpu6050的ID
* 返回：芯片ID
*/
uint8_t get_mpu_id(void)
{
uint8_t mpu_id;
mpu_id = IIC_Read_One_Byte(0xD0,WHO_AM_I);
return mpu_id;
}
/*
* 函数名：get_acc_raw
* 描述：读取加速度计三轴原始数据
* 输入：
* 返回：
*/
void get_acc_raw(void)
{
acc_raw.x = get_data(ACCEL_XOUT_H);
acc_raw.y = get_data(ACCEL_YOUT_H);
acc_raw.z = get_data(ACCEL_ZOUT_H);
acc_raw_f.x = (float)acc_raw.x;
acc_raw_f.y = (float)acc_raw.y;
acc_raw_f.z = (float)acc_raw.z;
}
/*
* 变量名：gyro_30hz_parameter
* 描述：巴特沃斯低通滤波器参数
* 输入：
* 返回：
*/
_Butterworth_parameter gyro_30hz_parameter =
{
//200hz---30hz
1, -0.7477891782585, 0.272214937925,
0.1311064399166, 0.2622128798333, 0.1311064399166
};
_Butterworth_data gyro_butter_data[3];
/*
* 函数名：get_gyro_raw
* 描述：读取陀螺仪三轴原始数据 & 进行校准 & 低通滤波
*/
void get_gyro_raw(void)
{
gyro_raw.x = get_data(GYRO_XOUT_H) - gyro_offset.x;
gyro_raw.y = get_data(GYRO_YOUT_H) - gyro_offset.y;
gyro_raw.z = get_data(GYRO_ZOUT_H) - gyro_offset.z;
gyro_raw_f.x = (float)butterworth_lpf(((float)gyro_raw.x),&gyro_butter_data[0],&gyro_30hz_parameter);
gyro_raw_f.y = (float)butterworth_lpf(((float)gyro_raw.y),&gyro_butter_data[1],&gyro_30hz_parameter);
gyro_raw_f.z = (float)butterworth_lpf(((float)gyro_raw.z),&gyro_butter_data[2],&gyro_30hz_parameter);
gyro_raw_cal.x = (float)get_data(GYRO_XOUT_H);
gyro_raw_cal.y = (float)get_data(GYRO_YOUT_H);
gyro_raw_cal.z = (float)get_data(GYRO_ZOUT_H);
}
/* get_iir_factor(&Mpu.att_acc_factor,0.005f,25);
* 描述：求取IIR滤波器的滤波因子
* 输入：out_factor滤波因子首地址，Time任务执行周期，Cut_Off滤波截止频率
*/
void get_iir_factor(float *out_factor,float Time, float Cut_Off)
{
*out_factor = Time /( Time + 1/(2.0f * PI * Cut_Off) );
}
//IIR低通滤波器(加速度)
void acc_iir_lpf(SI_F_XYZ *acc_in,SI_F_XYZ *acc_out,float lpf_factor)
{
acc_out->x = acc_out->x + lpf_factor*(acc_in->x - acc_out->x);
acc_out->y = acc_out->y + lpf_factor*(acc_in->y - acc_out->y);
acc_out->z = acc_out->z + lpf_factor*(acc_in->z - acc_out->z);
}
//加速度计滤波参数
_Butterworth_parameter acc_5hz_parameter =
{
1, -1.778631777825, 0.8008026466657,
0.005542717210281, 0.01108543442056, 0.005542717210281
};
_Butterworth_data acc_butter_data[3];
/*
* 描述：巴特沃斯加速度低通滤波
* 输入：acc_in滤波前的加速度，acc_out滤波后的输出加速度数据
*/
void acc_butterworth_lpf(SI_F_XYZ *acc_in,SI_F_XYZ *acc_out)
{
acc_out->x = butterworth_lpf(acc_in->x,&acc_butter_data[0],&acc_5hz_parameter);
acc_out->y = butterworth_lpf(acc_in->y,&acc_butter_data[1],&acc_5hz_parameter);
acc_out->z = butterworth_lpf(acc_in->z,&acc_butter_data[2],&acc_5hz_parameter);
}
/*
* 描述：原始加速度转为重力加速度g为单位数据
* 输入：acc_in原始的加速度，acc_out以g为单位的加速度数据
*/
void get_acc_g(SI_F_XYZ *acc_in,SI_F_XYZ *acc_out)
{
acc_out->x = (float)(acc_in->x * acc_raw_to_g);
acc_out->y = (float)(acc_in->y * acc_raw_to_g);
acc_out->z = (float)(acc_in->z * acc_raw_to_g);
}
/*
* 描述：原始陀螺仪数据转为弧度/秒为单位的数据
* 输入：gyro_in原始的陀螺仪数据，gyro_out以rad/s为单位的陀螺仪数据
*/
void get_rad_s(SI_F_XYZ *gyro_in,SI_F_XYZ *gyro_out)
{
gyro_out->x = (float)(gyro_in->x * gyro_raw_to_radian_s);
gyro_out->y = (float)(gyro_in->y * gyro_raw_to_radian_s);
gyro_out->z = (float)(gyro_in->z * gyro_raw_to_radian_s);
}
/*
* 描述：原始陀螺仪数据转为度/秒为单位的数据
* 输入：gyro_in原始的陀螺仪数据首地址，gyro_deg_out以deg/s为单位的陀螺仪数据首地址
*/
void get_deg_s(SI_F_XYZ *gyro_in,SI_F_XYZ *gyro_deg_out)
{
gyro_deg_out->x = (float)(gyro_in->x * gyro_raw_to_deg_s);
gyro_deg_out->y = (float)(gyro_in->y * gyro_raw_to_deg_s);
gyro_deg_out->z = (float)(gyro_in->z * gyro_raw_to_deg_s);
}

复制代码

oled.c

#include "oled.h"
#include "oledfont.h"
void IICO_Start()
{
OLED_SCLK_Set() ;
OLED_SDIN_Set();
OLED_SDIN_Clr();
OLED_SCLK_Clr();
}
void IICO_Stop()
{
OLED_SCLK_Set() ;
// OLED_SCLK_Clr();
OLED_SDIN_Clr();
OLED_SDIN_Set();
}
void IICO_Wait_Ack()
{
/* while(1)
{
if(!OLED_SDA)
{
return;
}
}
*/
OLED_SCLK_Set() ;
OLED_SCLK_Clr();
}
void Write_IIC_Byte(unsigned char IIC_Byte)
{
unsigned char i;
unsigned char m,da;
da=IIC_Byte;
OLED_SCLK_Clr();
for(i=0;i<8;i++)
{
m=da;
// OLED_SCLK_Clr();
m=m&0x80;
if(m==0x80)
{
OLED_SDIN_Set();
}
else
OLED_SDIN_Clr();
da=da<<1;
OLED_SCLK_Set();
OLED_SCLK_Clr();
}
}
void Write_IIC_Command(unsigned char IIC_Command)
{
IICO_Start();
Write_IIC_Byte(0x78); //Slave address,SA0=0
IICO_Wait_Ack();
Write_IIC_Byte(0x00); //write command
IICO_Wait_Ack();
Write_IIC_Byte(IIC_Command);
IICO_Wait_Ack();
IICO_Stop();
}
void Write_IIC_Data(unsigned char IIC_Data)
{
IICO_Start();
Write_IIC_Byte(0x78); //D/C#=0; R/W#=0
IICO_Wait_Ack();
Write_IIC_Byte(0x40); //write data
IICO_Wait_Ack();
Write_IIC_Byte(IIC_Data);
IICO_Wait_Ack();
IICO_Stop();
}
void OLED_WR_Byte(unsigned dat,unsigned cmd)
{
if(cmd)Write_IIC_Data(dat);
else Write_IIC_Command(dat);
}
//坐标设置
void OLED_Set_Pos(unsigned char x, unsigned char y)
{
OLED_WR_Byte(0xb0+y,OLED_CMD);
OLED_WR_Byte(((x&0xf0)>>4)|0x10,OLED_CMD);
OLED_WR_Byte((x&0x0f),OLED_CMD);
}
//开启OLED显示
void OLED_Display_On(void)
{
OLED_WR_Byte(0X8D,OLED_CMD); //SET DCDC命令
OLED_WR_Byte(0X14,OLED_CMD); //DCDC ON
OLED_WR_Byte(0XAF,OLED_CMD); //DISPLAY ON
}
//关闭OLED显示
void OLED_Display_Off(void)
{
OLED_WR_Byte(0X8D,OLED_CMD); //SET DCDCÃüÁî
OLED_WR_Byte(0X10,OLED_CMD); //DCDC OFF
OLED_WR_Byte(0XAE,OLED_CMD); //DISPLAY OFF
}
void OLED_Clear(void)
{
uint8_t i,n;
for(i=0;i<8;i++)
{
OLED_WR_Byte (0xb0+i,OLED_CMD);
OLED_WR_Byte (0x00,OLED_CMD);
OLED_WR_Byte (0x10,OLED_CMD);
for(n=0;n<128;n++)OLED_WR_Byte(0,OLED_DATA);
}
}
void OLED_On(void)
{
uint8_t i,n;
for(i=0;i<8;i++)
{
OLED_WR_Byte (0xb0+i,OLED_CMD); //设置页地址0-7
OLED_WR_Byte (0x00,OLED_CMD); //设置显示位置-列高地址
OLED_WR_Byte (0x10,OLED_CMD); //设置显示位置-列低地址
for(n=0;n<128;n++)OLED_WR_Byte(1,OLED_DATA);
}
}
//在指定位置显示一个字符
//x:0~127 y:0~63
//mode:0,反白显示;1,正常显示
//size:选择字体 16/12
void OLED_ShowChar(uint8_t x,uint8_t y,uint8_t chr,uint8_t Char_Size)
{
unsigned char c=0,i=0;
c=chr-' '; //得到偏移后的地址
if(x>Max_Column-1)
{
x=0;
y=y+2;
}
if(Char_Size ==16)
{
OLED_Set_Pos(x,y);
for(i=0;i<8;i++)
OLED_WR_Byte(F8X16[c*16+i],OLED_DATA);
OLED_Set_Pos(x,y+1);
for(i=0;i<8;i++)
OLED_WR_Byte(F8X16[c*16+i+8],OLED_DATA);
}
else
{
OLED_Set_Pos(x,y);
for(i=0;i<6;i++)
OLED_WR_Byte(F6x8[c][i],OLED_DATA);
}
}
uint32_t oled_pow(uint8_t m,uint8_t n)//m^n函数
{
uint32_t result=1;
while(n--)result*=m;
return result;
}
//显示两个数字
//x,y :起点坐标
//len :数字的位数
//size:字体大小
//mode:模式0,填充模式;1,叠加模式
//num:数值(0~4294967295);
void OLED_ShowNum(uint8_t x,uint8_t y,uint32_t num,uint8_t len,uint8_t size2)
{
uint8_t t,temp;
uint8_t enshow=0;
for(t=0;t<len;t++)
{
temp=(num/oled_pow(10,len-t-1))%10;
if(enshow==0&&t<(len-1))
{
if(temp==0)
{
OLED_ShowChar(x+(size2/2)*t,y,' ',size2);
continue;
}
else enshow=1;
}
OLED_ShowChar(x+(size2/2)*t,y,temp+'0',size2);
}
}
void OLED_ShowString(uint8_t x,uint8_t y,uint8_t *chr,uint8_t Char_Size) //显示一个字符号串
{
unsigned char j=0;
while (chr[j]!='\0')
{
OLED_ShowChar(x,y,chr[j],Char_Size);
x+=8;
if(x>120){x=0;y+=2;}j++;
}
}
void OLED_ShowCHinese(uint8_t x,uint8_t y,uint8_t no) //显示汉字
{
uint8_t t,adder=0;
OLED_Set_Pos(x,y);
for(t=0;t<16;t++)
{
OLED_WR_Byte(Hzk[2*no][t],OLED_DATA);
adder+=1;
}
OLED_Set_Pos(x,y+1);
for(t=0;t<16;t++)
{
OLED_WR_Byte(Hzk[2*no+1][t],OLED_DATA);
adder+=1;
}
}
void OLED_Init(void) //初始化SSD1306
{
// tdelay_ms(800);
OLED_WR_Byte(0xAE,OLED_CMD);//--display off
OLED_WR_Byte(0x00,OLED_CMD);//---set low column address
OLED_WR_Byte(0x10,OLED_CMD);//---set high column address
OLED_WR_Byte(0x40,OLED_CMD);//--set start line address
OLED_WR_Byte(0xB0,OLED_CMD);//--set page address
OLED_WR_Byte(0x81,OLED_CMD); // contract control
OLED_WR_Byte(0xFF,OLED_CMD);//--128
OLED_WR_Byte(0xA1,OLED_CMD);//set segment remap
OLED_WR_Byte(0xA6,OLED_CMD);//--normal / reverse
OLED_WR_Byte(0xA8,OLED_CMD);//--set multiplex ratio(1 to 64)
OLED_WR_Byte(0x3F,OLED_CMD);//--1/32 duty
OLED_WR_Byte(0xC8,OLED_CMD);//Com scan direction
OLED_WR_Byte(0xD3,OLED_CMD);//-set display offset
OLED_WR_Byte(0x00,OLED_CMD);//
OLED_WR_Byte(0xD5,OLED_CMD);//set osc division
OLED_WR_Byte(0x80,OLED_CMD);//
OLED_WR_Byte(0xD8,OLED_CMD);//set area color mode off
OLED_WR_Byte(0x05,OLED_CMD);//
OLED_WR_Byte(0xD9,OLED_CMD);//Set Pre-Charge Period
OLED_WR_Byte(0xF1,OLED_CMD);//
OLED_WR_Byte(0xDA,OLED_CMD);//set com pin configuartion
OLED_WR_Byte(0x12,OLED_CMD);//
OLED_WR_Byte(0xDB,OLED_CMD);//set Vcomh
OLED_WR_Byte(0x30,OLED_CMD);//
OLED_WR_Byte(0x8D,OLED_CMD);//set charge pump enable
OLED_WR_Byte(0x14,OLED_CMD);//
OLED_WR_Byte(0xAF,OLED_CMD);//--turn on oled panel
OLED_Clear() ;
}

复制代码

超声波检测距离与oled显示：

void SR04_OLED(void)
{
/* 超声波测量信号 */
dis_Start=1;
R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_10, 1);
R_BSP_SoftwareDelay (20, BSP_DELAY_UNITS_MICROSECONDS);
R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_10, 0);
/* OLED显示 */
OLED_ShowString(0,0,"ValLeft :",12);
if(hall1count>=0)
{
OLED_ShowString(82,0,"+",12);
OLED_ShowNum(90,0,hall1count,3,12);
}
else
{
OLED_ShowString(82,0,"-",12);
OLED_ShowNum(90,0,-(hall1count),3,12);
}
OLED_ShowString(0,1,"ValRight:",12);
if(hall2count>=0)
{
OLED_ShowString(82,1,"+",12);
OLED_ShowNum(90,1,hall2count,3,12);
}
else
{
OLED_ShowString(82,1,"-",12);
OLED_ShowNum(90,1,-(hall2count),3,12);
}
Dis=dis;
OLED_ShowString(0,6,"dis:",12);
OLED_ShowNum(90,6,Dis,3,12);
OLED_ShowString(0,4,"Mode:",12);
OLED_ShowNum(90,4,mode,3,12);
}
void SR04_callback(external_irq_callback_args_t *p_args)
{
timer_status_t status;//获取当前计数值
if (1 == p_args->channel)
{
/*读取端口电平状态，如果是低电平则发生的是下降沿，高电平则是上升沿*/
R_IOPORT_PinRead(&g_ioport_ctrl, BSP_IO_PORT_01_PIN_04, &p_port_value_port_104);
if(dis_Start == 1)
{
if(p_port_value_port_104 == BSP_IO_LEVEL_HIGH)//上升沿
{
R_GPT_CounterSet(&g_timer3_ctrl, 0);
R_GPT_Start(&g_timer3_ctrl);
}
else
{
dis_Start=0;
R_GPT_StatusGet(&g_timer3_ctrl, &status);
Hightime_counter =status.counter;
dis = (float)Hightime_counter*17000/48000000 ;//(高电平时间)*340*100/2 cm 高电平时间；高电平计数次数/PCLK(48M)
R_GPT_Stop(&g_timer3_ctrl);
}
}
}
}

复制代码

克哈keha · 发表于 2023-8-21 23:47:30

用到的资源：

克哈keha · 发表于 2023-8-27 23:34:24

不同参数的电机要更改不同的PID参数，甚至可能要调整定时器时间。
工程代码：

RA2E1_Car.zip (1.45 MB, 下载次数: 50)

想要学习平衡小车的，建议B站观看：（很详细，讲解很棒）
https://www.bilibili.com/video/B ... 99bc20cf9fbfd79c4c9

小板原理图，小板打板文件：

克哈RA2平衡车小板.pdf (204.78 KB, 下载次数: 20)

克哈RA2平衡车小板.zip (92.42 KB, 下载次数: 15)
防止盗卖行为，底板就不放上来了，有兴趣的直接按图做一个。

账号		自动登录	找回密码
密码			注册

【瑞萨RA MCU创意氛围赛】瑞萨RA蓝牙平衡小车

主题推荐