亲 xs128陀螺仪采集哪错了 跪求

余晖。。

#include <hidef.h>      /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include "PLL.H"
#include "PIT.H"
#include "PWM.H"
#include "SCI.H"
#include "ADC.H"
#include "IO.H"
#include "OUTPUTDATA.H"

word AdColle[6]={
0};
float  ADCOUT=20;
float AD_ACC_Z;
float AD_GYRO;
int iz;
float faccz=0xcc;
float fgyroz=0xc8;
float facc,fgyro,fg,faccspeek,fCarAngle,angle_m,gyro_m;
float fgyroaccIntegral=0;
float ffValue,fAngleControlOut;
float fLeftMotorOut = 0 ;
float fRightMotorOut =0;
float g_fGravityAngle ;
float angle,angle_dot;


volatile byte PIT_CNT=0;


void fAD_QIUHE (void)
{
   float temp8B;
   
  
   for(temp8B = 0 ; temp8B < 20 ;temp8B ++){
       AdColle[1] +=AD_Measure12(10) ;       //AR1
   }
    for(temp8B = 0 ; temp8B <20 ;temp8B ++){
       AdColle[2] +=AD_Measure12(11)  ;       //ZG
   }
   
  facc=AdColle[2]/20;
  fgyro=AdColle[1]/20;
  
}

/*void fAD_Average(void)
  {
    Average[0]=AdColle[0]/ADCOUT;
    Average[1]=AdColle[1]/ADCOUT;
    Average[2]=AdColle[2]/ADCOUT;
    Average[3]=AdColle[3]/ADCOUT;
    Average[4]=AdColle[4]/ADCOUT;
    AdColle[0]=0;
    AdColle[1]=0;
    AdColle[2]=0;
    AdColle[3]=0;
    AdColle[4]=0;
    facc=Average[2];
    fgyro=Average[1] ;
  } */
            
                           
//-------------------角度计算-----------------------//
void  AngleCalculate ()     //角度计算
   {
         
         float fDeltaValue;
         fg=(fgyro -fgyroz) *0.5;    ///15
         faccspeek=(facc-faccz) *1.118;    ///2.7   
          if(faccspeek>90)  faccspeek=90;
          if(faccspeek<-90) faccspeek=-90;                                          
         
        fCarAngle=fgyroaccIntegral;
        fDeltaValue =(faccspeek- fCarAngle) / 4;      
         fgyroaccIntegral += ( fg + fDeltaValue)  * 200;        ////0.005
                    
}
//----------------------直立角度计算函数-----------------//
  void AngleControl ()
  {  
                                       
     ffValue =(0-fCarAngle) *0.5+(0-fg) *0;//0.20
      fAngleControlOut = ffValue;
      ffValue=0;
  
  }   
///*---------------电机输出------*/
  void MotoOutput()
  {
     float fLeft, fRight;

     fLeft  =fAngleControlOut;
     fRight =fAngleControlOut;
     if(fLeft >0)   fLeft+=3;
   
     if(fRight>0)   fRight+=3;
   
     if(fLeft <0)   fLeft-=3;
   
     if(fRight<0)  fRight-=3;
   
     if(fLeft >= 150)    fLeft=100;
     
     if(fLeft <= -150)  fLeft=-100;
   
     if(fRight >= 150)   fRight= 100;
   
     if(fRight <= -150)  fRight=-100;
   
     fLeftMotorOut =  fLeft ;
     fRightMotorOut = fRight;     
    if(fLeft>= 0)
       {  //左轮输出
          PWMDTY7=fLeft;   
          PWMDTY6=0;
       }
    else
      {
          fLeft=-fLeft;
          PWMDTY7=0;   
          PWMDTY6=fLeft;
      }
  
    if (fRight>=0)
     {  //右轮输出
          PWMDTY5=fRight;   
          PWMDTY4=0;  
     }               
  else
     {
         fRight=-fRight;
         PWMDTY5=0;   
         PWMDTY4=fRight;  
     }
  
}


void main(void)
{
  
   
  
    PLL_Init();
    SCI_Init();
    PWM_Init () ;
    IO_Init()  ;
    PIT_Init();
    ADC0_Init() ;
    OutPut_Data();
   
    EnableInterrupts;

    for(;;) {
    OutData[0]=fgyro; ;
    OutData[1]=facc;
    //OutData[2]=fAngleControlOut;
    OutPut_Data();
   //  _FEED_COP(); /* feeds the dog   */
    }
   
}
      
   


#pragma CODE_SEG __NEAR_SEG NON_BANKED  //指示该程序在不分页区
void interrupt 66 PIT0 (void)
{   
     
   
   
   iz++;
  
         
   
   if(iz>=5)
      {  
        
         iz = 0;  
      }
   else if (iz==1)
     {   
        fAD_QIUHE();
      //  fAD_Average();
       SCI0_SendChar1((char)fgyro>>8);
       SCI0_SendChar1((char)fgyro);
      }
   else if(iz==2)
     {
         AngleCalculate ();
         AngleControl();
         MotoOutput();        
     }
     
      
     PITTF_PTF0=1;
}

alxi44

你发的那个帖子是xs128陀螺仪调试程序吧？

余晖。。

恩   加上我自己编的