完美的平衡车程序，完整的代码，有卡尔曼滤波和PID
单片机源程序如下:
- /***********************************************
- 标题: mian.c
- 日期: 2013/12/27
- 版本：v1.0
- MDK-ARM 版本: v4.12
- ST 库版本：v3.50
- 功能: 自平衡小车平衡及控制
- 说明：可结合上位机校正六轴的补偿值，使用蓝牙或者NRF24L01控制
- *************************************************/
- #include "STM32f10x.h"
- #include "iic.h"
- #include "timer.h"
- #include "usart.h"
- #include "24l01.h"
- #include "mpu3050.h"
- #include "adxl345.h"
- #include "filter.h"
- #include "calculate.h"
- #include "u***_lib.h"
- #include "u***_desc.h"
- #include "u***_istr.h"
- #include "hw_config.h"
- #include "u***_pwr.h"
- #include "u***_pwr.h"
- #include
- #include
- u8 receive_data;
- u8 flg_get_senor_data;
- u8 out[35] ={0x5f, 0x60, 0};
- static float angle, angle_dot, f_angle, f_angle_dot;
- s16 temp;
- s16 gx, gy, gz, ax ,ay, az, temperature;
- s16 gx_offset, ax_offset, ay_offset, az_offset;
- u8 tmp[2]={0};
- #define USB
- #define FILTER_COUNT 32
- s16 gx_buf[FILTER_COUNT], ax_buf[FILTER_COUNT], ay_buf[FILTER_COUNT],az_buf[FILTER_COUNT];
- /******************************************************************************/
- void delay(u32 count) //延时函数
- {
- for(; count != 0; count--);
- }
- /*************************************************
- 名称：void acc_filter(void)
- 功能：加速度计数据滤波
- 输入参数：据滤波后的数据
- 输出参数：无
- 返回值： 无
- **************************************************/
- void acc_filter(void)
- {
- u8 i;
- s32 ax_sum = 0, ay_sum = 0, az_sum = 0;
- for(i = 1 ; i < FILTER_COUNT; i++)
- {
- ax_buf[i - 1] = ax_buf;
- ay_buf[i - 1] = ay_buf;
- az_buf[i - 1] = az_buf;
- }
- ax_buf[FILTER_COUNT - 1] = ax;
- ay_buf[FILTER_COUNT - 1] = ay;
- az_buf[FILTER_COUNT - 1] = az;
- for(i = 0 ; i < FILTER_COUNT; i++)
- {
- ax_sum += ax_buf;
- ay_sum += ay_buf;
- az_sum += az_buf;
- }
- ax = (s16)(ax_sum / FILTER_COUNT);
- ay = (s16)(ay_sum / FILTER_COUNT);
- az = (s16)(az_sum / FILTER_COUNT);
- }
- /*****************************************************************************/
- int main(void)
- {
- GPIO_InitTypeDef GPIO_InitStructure;
- #ifdef USB //u***接入
- NVIC_InitTypeDef NVIC_InitStructure;
- #endif
- u8 j, i=0;
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB, ENABLE);
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOA, &GPIO_InitStructure);
- GPIO_SetBits(GPIOA,GPIO_Pin_5); //blue led
- #ifdef USB
- u***_system_init();
- #endif
- //初始化
- usart_init();
- iic_init();
- timer_init();
- NRF24L01_Init(); //初始化NRF24L01
- mpu3050_init();
- adxl345_init();
- motor_init();
- LED2OFF;//确认关闭LED2
- while(NRF24L01_Check())//检测不到24L01
- {
- delay(3000);
- LED2ON; //LED2常亮
- }
- RX_Mode(); //24L01接收模式
- LED1ON;
- while (1)
- {
- if(flg_get_senor_data) //TIM3触发
- {
- flg_get_senor_data = 0;
- #ifdef USB
- NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; //USB通讯关闭
- NVIC_Init(&NVIC_InitStructure);
- #endif
- //获取六轴的原始值
- mpu3050_get_data(&gx, &gy, &gz, &temperature);
- adxl345_get_data(&ax, &ay ,&az);
- #ifdef USB
- NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //USB通讯启动
- NVIC_Init(&NVIC_InitStructure);
- #endif
- acc_filter();
- gx -= gx_offset; //gx补偿校正
- ax -= ax_offset; //ax补偿校正
- ay -= ay_offset; //ay补偿校正
- az -= az_offset; //az补偿校正
- angle_dot = gx * GYRO_SCALE; //+-2000 0.060975 °/LSB
- angle = atan(ay / sqrt(ax * ax + az * az ));
- angle = angle * 57.295780; //180/pi
- kalman_filter(angle, angle_dot, &f_angle, &f_angle_dot);//卡尔曼滤波函数
- if(tmp[0]==0){ receive_parameter(receive_data);}//蓝牙控制
- //---------------------------------------------------------------
- if(NRF24L01_RxPacket(tmp)==0) //24L01控制
- {
- receive_parameter(tmp[1]);
- LED1OFF;
- }
- //------------------------------------------------------------
- pid(f_angle, f_angle_dot); //PID调节
- #ifdef USB //USB上位机数据处理
- temp = (s16)(f_angle * 100);
- out[2] = (u8)(gx >> 8);
- out[3] = (u8)(gx);
- out[4] = (u8)(gy >> 8);
- out[5] = (u8)(gy);
- out[6] = (u8)(gz >> 8);
- out[7] = (u8)(gz);
- out[8] = (u8)(ax >> 8);
- out[9] = (u8)(ax);
- out[10] = (u8)(ay >> 8);
- out[11] = (u8)(ay);
- out[12] = (u8)(az >> 8);
- out[13] = (u8)(az);
- out[14] = (u8)(temp >> 8);
- out[15] = (u8)(temp);
- ……………………
- …………限于本文篇幅 余下代码请从电子发烧友下载附件…………
所有资料51hei提供下载:
 balance car nrf24l01程序 完美.rar
|
