|
前段时间搞了个平衡车,涉及stm32F3 步进电机驱动 陀螺仪mpu3050 加速度计adxl345(也可以用6轴mpu6050) 无线NRF24L01
当初最大问题是卡尔曼滤波(进行陀螺仪与加速度计的数据融合)和pid调节
对于卡尔曼滤波,经过自己不断深究,其实也不是很复杂,核心是五大公式,涉及矩阵运算,思想是预测值 最优估计值 噪声 协方差的概念,难点:一些参数选择
说下用卡尔曼滤波的出发点,陀螺仪 加速度计都可以得到角度,而陀螺仪是先得到角速度再经积分才得到角度, 陀螺仪相比加速度计短时间内动态性能好,得到角度精准,但本身有小漂移,随着时间变长,不断积分,误差会越来越大,那就需要用加速度计进行校正
对于pid算法,里面涉及二级pid,首先要明白小车速度跟给步进电机的频率是成正比的,就把频率等效为速度
第一个pid,角度pid,通过测角度反馈给stm32f3产生频率(速度)来进行平衡调节(即调节角度)
第二个pid,速度pid,由于角度调节产生了速度变化,而为了不改变设定的速度,需要进行速度调节,它的反馈来自不断角度pid的结果(由于速度跟频率成正比,不需要测速反馈)
难点:pid整定参数
- /***********************************************
- 标题: 24L01.c
- 日期: 2013/12/27
- 版本:v1.0
- 功能: 初始化以spi及数据读写
- 说明:24l01的初始化及spi2的调用
- 注意:在24l01.h中的管脚配置
- *************************************************/
- #include "stm32f10x.h"
- #include "24l01.h"
- #include "spi.h"
- #include
-
- const u8 TX_ADDRESS[TX_ADR_WIDTH]={0x34,0x43,0x10,0x10,0x01}; //发送地址
- const u8 RX_ADDRESS[RX_ADR_WIDTH]={0x34,0x43,0x10,0x10,0x01}; //发送地址
-
- //初始化24L01的IO口
- void NRF24L01_Init(void)
- {
-
- GPIO_InitTypeDef GPIO_InitStructure;
- RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOD, ENABLE );
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;//CE PB12
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ; //推挽输出
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
- GPIO_SetBits(GPIOB,GPIO_Pin_12);
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_2|GPIO_Pin_3;//LSN
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ; //推挽输出
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOD, &GPIO_InitStructure);
- GPIO_SetBits(GPIOD,GPIO_Pin_8);
-
- GPIO_SetBits(GPIOD,GPIO_Pin_2);//LED1
- GPIO_SetBits(GPIOD,GPIO_Pin_3);//LED2
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU ; //上拉输入
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOD, &GPIO_InitStructure);
- SPI2_Init(); //初始化SPI2
-
- Clr_NRF24L01_CE; //使能24L01 NRF24L01_CE
- Set_NRF24L01_CSN; //SPI片选取消 NRF24L01_CSN
- }
- //检测24L01是否存在
- //返回值:0,成功;1,失败
- u8 NRF24L01_Check(void)
- {
- u8 buf[5]={0XA5,0XA5,0XA5,0XA5,0XA5};
- u8 i;
- NRF24L01_Write_Buf(NRF24L01_WRITE_REG+TX_ADDR,buf,5);//写入5个字节的地址.
- NRF24L01_Read_Buf(TX_ADDR,buf,5); //读出写入的地址
- for(i=0;i<5;i++)if(buf[i]!=0XA5)break;
- if(i!=5)return 1;//检测24L01错误
- return 0; //检测到24L01
- }
-
- //SPI写寄存器
- //reg:指定寄存器地址
- //value:写入的值
- u8 NRF24L01_Write_Reg(u8 reg,u8 value)
- {
- u8 status;
- Clr_NRF24L01_CSN; //使能SPI传输
- status =SPIx_ReadWriteByte(reg);//发送寄存器号
- SPIx_ReadWriteByte(value); //写入寄存器的值
- Set_NRF24L01_CSN; //禁止SPI传输
- return(status); //返回状态值
- }
- //读取SPI寄存器值
- //reg:要读的寄存器
- u8 NRF24L01_Read_Reg(u8 reg)
- {
- u8 reg_val;
- Clr_NRF24L01_CSN; //使能SPI传输
- SPIx_ReadWriteByte(reg); //发送寄存器号
- reg_val=SPIx_ReadWriteByte(0XFF);//读取寄存器内容
- Set_NRF24L01_CSN; //禁止SPI传输
- return(reg_val); //返回状态值
- }
- //在指定位置读出指定长度的数据
- //reg:寄存器(位置)
- //*pBuf:数据指针
- //len:数据长度
- //返回值,此次读到的状态寄存器值
- u8 NRF24L01_Read_Buf(u8 reg,u8 *pBuf,u8 len)
- {
- u8 status,u8_ctr;
- Clr_NRF24L01_CSN; //使能SPI传输
- status=SPIx_ReadWriteByte(reg);//发送寄存器值(位置),并读取状态值
- for(u8_ctr=0;u8_ctr<len;u8_ctr++)pbuf[u8_ctr]=spix_readwritebyte(0xff); 读出数据
- Set_NRF24L01_CSN; //关闭SPI传输
- return status; //返回读到的状态值
- }
- //在指定位置写指定长度的数据
- //reg:寄存器(位置)
- //*pBuf:数据指针
- //len:数据长度
- //返回值,此次读到的状态寄存器值
- u8 NRF24L01_Write_Buf(u8 reg, u8 *pBuf, u8 len)
- {
- u8 status,u8_ctr;
- Clr_NRF24L01_CSN; //使能SPI传输
- status = SPIx_ReadWriteByte(reg);//发送寄存器值(位置),并读取状态值
- for(u8_ctr=0; u8_ctr<len; u8_ctr++)spix_readwritebyte(*pbuf++);="" 写入数据=""
- Set_NRF24L01_CSN; //关闭SPI传输
- return status; //返回读到的状态值
- }
-
- //启动NRF24L01发送一次数据
- //txbuf:待发送数据首地址
- //返回值:发送完成状况
- u8 NRF24L01_TxPacket(u8 *txbuf)
- {
- u8 sta;
- Clr_NRF24L01_CE;
- NRF24L01_Write_Buf(NRF24L01_WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);//写数据到TX BUF 32个字节
- Set_NRF24L01_CE;//启动发送
- while(NRF24L01_IRQ!=0);//等待发送完成
- sta=NRF24L01_Read_Reg(STATUS); //读取状态寄存器的值
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中断标志
- if(sta&MAX_TX)//达到最大重发次数
- {
- NRF24L01_Write_Reg(NRF24L01_FLUSH_TX,0xff);//清除TX FIFO寄存器
- return MAX_TX;
- }
- if(sta&TX_OK)//发送完成
- {
- return TX_OK;
- }
- return 0xff;//其他原因发送失败
- }
- //启动NRF24L01发送一次数据
- //txbuf:待发送数据首地址
- //返回值:0,接收完成;其他,错误代码
- u8 NRF24L01_RxPacket(u8 *rxbuf)
- {
- u8 sta;
- sta=NRF24L01_Read_Reg(STATUS); //读取状态寄存器的值
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中断标志
- if(sta&RX_OK)//接收到数据
- {
- NRF24L01_Read_Buf(NRF24L01_RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);//读取数据
- NRF24L01_Write_Reg(NRF24L01_FLUSH_RX,0xff);//清除RX FIFO寄存器
- return 0;
- }
- return 1;//没收到任何数据
- }
-
- //该函数初始化NRF24L01到RX模式
- //设置RX地址,写RX数据宽度,选择RF频道,波特率和LNA HCURR
- //当CE变高后,即进入RX模式,并可以接收数据了
- void RX_Mode(void)
- {
- Clr_NRF24L01_CE;
- NRF24L01_Write_Buf(NRF24L01_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH);//写RX节点地址
-
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01); //使能通道0的自动应答
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01);//使能通道0的接收地址
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40); //设置RF通信频率
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);//选择通道0的有效数据宽度
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f);//设置TX发射参数,0db增益,2Mbps,低噪声增益开启
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG, 0x0f);//配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,接收模式
- Set_NRF24L01_CE; //CE为高,进入接收模式
- }
-
- //该函数初始化NRF24L01到TX模式
- //设置TX地址,写TX数据宽度,设置RX自动应答的地址,填充TX发送数据,选择RF频道,波特率和LNA HCURR
- //PWR_UP,CRC使能
- //当CE变高后,即进入RX模式,并可以接收数据了
- //CE为高大于10us,则启动发送.
- void TX_Mode(void)
- {
- Clr_NRF24L01_CE;
- NRF24L01_Write_Buf(NRF24L01_WRITE_REG+TX_ADDR,(u8*)TX_ADDRESS,TX_ADR_WIDTH);//写TX节点地址
- NRF24L01_Write_Buf(NRF24L01_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); //设置TX节点地址,主要为了使能ACK
-
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01); //使能通道0的自动应答
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01); //使能通道0的接收地址
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+SETUP_RETR,0x1a);//设置自动重发间隔时间:500us + 86us;最大自动重发次数:10次
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40); //设置RF通道为40
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f); //设置TX发射参数,0db增益,2Mbps,低噪声增益开启
- NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG,0x0e); //配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,接收模式,开启所有中断
- Set_NRF24L01_CE;//CE为高,10us后启动发送
- }
复制代码
代码资料(完美)见下
balance car nrf24l01程序 完美.zip
(6.43 MB, 下载次数: 241)
平衡车卡尔曼滤波资料.zip
(1.2 MB, 下载次数: 209)
|
评分
-
查看全部评分
|