陀螺仪传感器融合_code例程.c

ENC-03M陀螺仪模块

 /* 
  * 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*/