完美的平衡車程序卡爾曼濾波和PID及nrf24l01無線

ID:255506 · 發(fā)表于 2017-11-30 08:34

完美的平衡車程序，完整的代碼，有卡爾曼濾波和PID

單片機源程序如下:

/***********************************************
標(biāo)題: mian.c
日期: 2013/12/27
版本：v1.0
MDK-ARM 版本: v4.12
ST 庫版本：v3.50
功能: 自平衡小車平衡及控制
說明：可結(jié)合上位機校正六軸的補償值，使用藍(lán)牙或者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 "usb_lib.h"
#include "usb_desc.h"
#include "usb_istr.h"
#include "hw_config.h"
#include "usb_pwr.h"
#include "usb_pwr.h"
#include <stdio.h>
#include <math.h>
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) //延時函數(shù)
{
for(; count != 0; count--);
}
/*************************************************
名稱：void acc_filter(void)
功能：加速度計數(shù)據(jù)濾波
輸入?yún)?shù)：據(jù)濾波后的數(shù)據(jù)
輸出參數(shù)：無
返回值：無
**************************************************/
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[i];
ay_buf[i - 1] = ay_buf[i];
az_buf[i - 1] = az_buf[i];
}
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[i];
ay_sum += ay_buf[i];
az_sum += az_buf[i];
}
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 //usb接入
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
usb_system_init();
#endif
//初始化
usart_init();
iic_init();
timer_init();
NRF24L01_Init(); //初始化NRF24L01
mpu3050_init();
adxl345_init();
motor_init();
LED2OFF;//確認(rèn)關(guān)閉LED2
while(NRF24L01_Check())//檢測不到24L01
{
delay(3000);
LED2ON; //LED2常亮
}
RX_Mode(); //24L01接收模式
LED1ON;
while (1)
{
if(flg_get_senor_data) //TIM3觸發(fā)
{
flg_get_senor_data = 0;
#ifdef USB
NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; //USB通訊關(guān)閉
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);//卡爾曼濾波函數(shù)
if(tmp[0]==0){ receive_parameter(receive_data);}//藍(lán)牙控制
//---------------------------------------------------------------
if(NRF24L01_RxPacket(tmp)==0) //24L01控制
{
receive_parameter(tmp[1]);
LED1OFF;
}
//------------------------------------------------------------
pid(f_angle, f_angle_dot); //PID調(diào)節(jié)
#ifdef USB //USB上位機數(shù)據(jù)處理
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);
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復(fù)制代碼

所有資料51hei提供下載:

代碼.7z (698.94 KB, 下載次數(shù): 119)

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

完美的平衡車程序 卡爾曼濾波和PID及nrf24l01無線

完美的平衡車程序卡爾曼濾波和PID及nrf24l01無線