|
|
楼主 |
发表于 2010-5-2 19:33:08
|
显示全部楼层
一、传感器融合
(原文件名:传感器融合.jpg)
1、图中实线部分为实时计算获取倾角值，虚线部分是计算陀螺仪偏差。s1、s2每0.5s闭合一次
2、‘放大’‘缩小’其目的是化成同一个数量级。每获取一度倾角，陀螺仪积分值约为1000，加速度计变化10/9左右。
二者相差约900倍。
3、另有温漂消除部分。思路是计算陀螺仪原始值及输出倾角在0.5s内的平均值，做差，据此来修正陀螺仪中值
二、主程序流程
1、等待2ms定时结束（自平衡控制周期为2ms）
2、获取最新传感器数据
3、传感器融合
4、PID
5、更新马达值
源程序：
/*
* Self-balancing Ctrl
*
* By DDAI
* 2010.03.21
*/
#include "system.h" //FPGA NIOS II 软核头文件
#include "sys/alt_irq.h"
#include "sys/alt_dev.h"
#include "alt_types.h"
#include "altera_avalon_pio_regs.h"
#include "altera_avalon_timer_regs.h"
#include "altera_avalon_timer.h"
#include "stdio.h"
alt_u8 data_acc; //read from ad
alt_u8 data_gyr;
alt_u8 middle_gyr;
/*Roll*/
alt_8 Reading_GyrRoll;
alt_8 Reading_AccRoll;
alt_16 Mean_AccRoll=512*12;
alt_32 Reading_IntegralGyrRoll; //Get degree
alt_32 Reading_IntegralGyrRoll_2;
alt_32 IntegralGyrRoll;
alt_32 IntegralGyrRoll_2;
alt_32 IntegralAccRoll;
alt_32 Mean_IntegralGyrRoll;
alt_32 IntegralDegreeRoll;
alt_32 IntegralErrorRoll;
alt_16 AuttitudecorrectionRoll = 0;
alt_32 Correction_Roll;
static int balance_number;
int timer_periodh,timer_periodl;
int balance_ctrl = 1;
int run = 0;
float Amplify = 12;
float Programset_GyrAcc = 32;
#define MIDDLE_ACC 129
#define P 1500
#define I 10
#define D 500
void init(){ //初始化 获取陀螺仪初值
int cnt=0;
alt_u16 init_gyr=0;
while(cnt<100){
data_gyr=IORD_16DIRECT(TLC549_IC_BASE+2*1,0);
init_gyr+=data_gyr;
cnt++;
}
middle_gyr = init_gyr/100;
}
void gui(){ //在nios2-terminal中输出倾角值
static int gui_cnt=0; //便于JAVA GUI 调用显示
gui_cnt++;
if(gui_cnt>500){
printf("%d\n",IntegralGyrRoll/255);
gui_cnt=0;
}
}
static void Interrupt_timer(void *context,alt_u32 id){ //定时器中断
balance_ctrl = 1;
IOWR_ALTERA_AVALON_TIMER_STATUS(TIMER_BASE,0);
}
void Init_timer(){ //定时器初始化
alt_irq_register(TIMER_IRQ,NULL,Interrupt_timer);
IOWR_ALTERA_AVALON_TIMER_PERIODH(TIMER_BASE,timer_periodh);
IOWR_ALTERA_AVALON_TIMER_PERIODL(TIMER_BASE,timer_periodl);
IOWR_ALTERA_AVALON_TIMER_CONTROL(TIMER_BASE,0x07);
}
int main(void){
alt_u8 data_acc_old; //low-pass fliter
alt_u8 data_gyr_old; //滤波器没有用到
int integral;
int degree,v;
int err2,err_old;
init();
timer_periodh = 1; //2ms 定时
timer_periodl = 0x86A0;
Init_timer();
for(;;){ //主循环
while(balance_ctrl){
balance_ctrl = 0;
data_acc=IORD_16DIRECT(TLC549_IC_BASE,0); //由IP核读取传感器值
data_gyr=IORD_16DIRECT(TLC549_IC_BASE+2*1,0);
/*LOW PASS FLITER*/
/*
data_acc = data_acc*71/100 + data_acc_old*29/100;
data_gyr = data_gyr*71/100 + data_gyr_old*29/100;
data_acc_old = data_acc;
data_gyr_old = data_gyr;
*/
/*Get sensors' value which has substricted by Middle value*/
Reading_AccRoll = data_acc-MIDDLE_ACC;
Reading_GyrRoll = data_gyr-middle_gyr;
gui();
/* Renew data */
Mean_AccRoll = (Mean_AccRoll + Amplify*Reading_AccRoll)/2;
IntegralAccRoll += Reading_AccRoll*Amplify; //Be used for long-time control
Reading_IntegralGyrRoll += (Reading_GyrRoll-AuttitudecorrectionRoll); //Real-time control
Reading_IntegralGyrRoll_2 += Reading_GyrRoll; //Original data integral
IntegralGyrRoll = Reading_IntegralGyrRoll; //Output tilt-Roll
IntegralGyrRoll_2 = Reading_IntegralGyrRoll_2;
Mean_IntegralGyrRoll += IntegralGyrRoll; //Long-time control
/*********************************************************************/
/* Real-time control */
/*********************************************************************/
Correction_Roll = ((IntegralGyrRoll/Programset_GyrAcc) - Mean_AccRoll); //Error signal
Correction_Roll /= 2;
#define MaxCorrection 64
if(Correction_Roll >= MaxCorrection) Correction_Roll = MaxCorrection;
else if(Correction_Roll <= -MaxCorrection) Correction_Roll = -MaxCorrection;
Reading_IntegralGyrRoll -= Correction_Roll;
/*********************************************************************/
/* Long-time control */
/*********************************************************************/
#define BAL_NUM 250
balance_number++;
if(balance_number >= BAL_NUM) //0.5s
{
alt_16 long_correct;
Mean_IntegralGyrRoll /= BAL_NUM;
IntegralAccRoll /= BAL_NUM;
long_correct = (Mean_IntegralGyrRoll - IntegralAccRoll);
AuttitudecorrectionRoll = (long_correct/BAL_NUM);
IntegralAccRoll = 0;
//printf("AuttitudecorrectionRoll %d ",AuttitudecorrectionRoll); //调试时使用
//printf("AuttitudecorrectionNick %d \n",AuttitudecorrectionNick);
/*********************************************************************/
/* 修正陀螺仪温漂 */
/*********************************************************************/
IntegralErrorRoll = IntegralGyrRoll_2 - IntegralGyrRoll;
Reading_IntegralGyrRoll_2 -= IntegralErrorRoll;
if(IntegralErrorRoll >= 4*balance_number) middle_gyr += 1;
if(IntegralErrorRoll <= -4*balance_number) middle_gyr -= 1;
balance_number = 0;
}
/*----------------------------------------------
* PID
* -------------------------------------------*/
degree = IntegralGyrRoll/255; //缩小倾角值，与马达匹配
err2=degree-err_old;
err_old=degree;
integral+=degree;
if(integral<-4999) integral=-4999; //溢出处理
if(integral>4999) integral=4999;
v=P*degree+D*err2+I*integral;
if(v>0){
if(v>4999) v=4999;
}
else{
v=5000-v;
if(v>9999) v=9999;
}
IOWR_16DIRECT(MOTOR_CTRL_0_BASE,0,v); //更新马达值
IOWR_16DIRECT(MOTOR_CTRL_1_BASE,0,v); //v取0~4999 马达正转 5000~9999 马达反转
}
}
}
/*end*/ |
|
发表于 2010-4-6 15:41:00
