加速度积分求速度和位移的c语言算法程序

#include
#include

#define uchar unsigned char
#define uint unsigned int

sfr P2M0  = 0x95;
sfr P2M1  = 0x96;
sfr CLK_DIV = 0xC7;

sbit SDA=P2^7;	
sbit SCL=P2^6;
sbit test=P2^5;
sbit test1=P2^2;

//-------------------变量初始化-------------------------------
uint ZuoBiao[2];    //存XY加速度值	
uint s[4];         //分别存XY加速度的高低位.
uint ack;
int iAccReal[4]={0000};
int iAccFilter;
int count=0;
double ax[4]={0.00.00.00.0}ay[4]={0.00.00.00.0}vx[3]={0.00.00.0}vy[3]={0.00.00.0};
double LX[2]={0.00.0}LY[2]={0.00.0};
double ax_temp=0.0ay_temp=0.0vx_temp=0.0vy_temp=0.0;
double t=0.005926;
//-------------------函数声明----------------------------------
void Start_I2c（）;
void Stop_I2c（）;
void SendByte（uchar c）;
uchar RcvByte（）;
void Ack_I2c（int a）;
void MXC_init（）;
void Delay1us（unsigned int us）;
void RS232_init（void）;
void serial_send（uchar dat）;
								
//-------------------函数定义----------------------------------
void main（void）
{  
    P2M0=0xC0; //开漏模式0xC0
    P2M1=0xC0; 
    CLK_DIV=0x00; 
   	TMOD=0x21;//0010 0001
				TH0=0x00;//定时
				TL0=0x00;
				TR0=1;//开启T0定时器
				ET0=1;//允许T0定时器中断
				EA=1;//开启总中断允许				 
	   RS232_init（）;	//串口初始化
				MXC_init（）;  //传感器初始化      
   while（1）
   {test=1;test1=0;Delay1us（1）;//测试中断是否执行，中断时间是否超出TH0/TL0的设定值
   }
}	

//-------------------IIC的开始命令-----------------------------	
void Start_I2c（）
{
   SDA=1;     /*发送起始条件的数据信号*/
   Delay1us（1）;
   SCL=1;
   Delay1us（2）;      /*起始条件建立时间大于4.7us延时*/  
   SDA=0;     /*发送起始信号*/
   Delay1us（2）;      /* 起始条件锁定时间大于4μs*/    
   SCL=0;     /*钳住I2C总线，准备发送或接收数据 */
   Delay1us（2）;
}
//-------------------IIC的停止命令-----------------------------
void Stop_I2c（）
{
   SDA=0;    /*发送结束条件的数据信号*/
   Delay1us（2）;     /*发送结束条件的时钟信号*/
   SCL=1;    /*结束条件建立时间大于4us*/ 
   Delay1us（2）;
   SDA=1;    /*发送I2C总线结束信号*/
   Delay1us（2）;
}
//------------------发送命令的函数-----------------------------
void SendByte（uchar c）
{
   uchar BitCnt;
   ack=0;
   for（BitCnt=0;BitCnt<8;BitCnt++）   /*要传送的数据长度为8位*/
   {
	    if（（c<	    else   SDA=0;                
	    Delay1us（1）;
	    SCL=1;                /*置时钟线为高，通知被控器开始接收数据位*/      
	    Delay1us（2）;              /*保证时钟高电平周期大于4μs*/            
	    SCL=0; 
   }   
	  Delay1us（2）;
	  SDA=1;                 /*8位发送完后释放数据线，准备接收应答位*/
	  Delay1us（2）;   
	  SCL=1;
	  Delay1us（2）;
	  if（SDA==1）ack=0;     
	  else ack=1;         /*判断是否接收到应答信号*/
	  SCL=0;
	  Delay1us（2）;
}
//------------------接收一次数据的函数-------------------------	
uchar RcvByte（）
{
   uchar retc;
   uchar BitCnt;
   retc=0; 
   SDA=1;                /*置数据线为输入方式*/
   for（BitCnt=0;BitCnt<8;BitCnt++）
    {
      Delay1us（1）;   
      SCL=0;                   /*置时钟线为低，准备接收数据位*/   
      Delay1us（2）;                  /*时钟低电平周期大于4.7μs*/   
      SCL=1;                   /*置时钟线为高使数据线上数据有效*/
      Delay1us（2）;
      retc=retc<<1;
      if（SDA==1）retc=retc