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

#include<reg51.h> #include<math.h> #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]={0,0,0,0}; int iAccFilter; int count=0; double ax[4]={0.0,0.0,0.0,0.0},ay[4]={0.0,0.0,0.0,0.0},vx[3]={0.0,0.0,0.0},vy[3]={0.0,0.0,0.0}; double LX[2]={0.0,0.0},LY[2]={0.0,0.0}; double ax_temp=0.0,ay_temp=0.0,vx_temp=0.0,vy_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<<BitCnt)&0x80)SDA=1; /*判断发送位*/ 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+1; /*读数据位,接收的数据位放入retc中 */ Delay1us(2); } SCL=0; Delay1us(2); return(retc); } //-------------------IIC协议的应答------------------------------ void Ack_I2c(int a) { if(a==0)SDA=0; /*在此发出应答或非应答信号 */ else SDA=1; Delay1us(2); SCL=1; Delay1us(2); /*时钟低电平周期大于4μs*/ SCL=0; /*清时钟线，钳住I2C总线以便继续接收*/ Delay1us(2); } //-------------------传感器初始化--------------------------------- void MXC_init() { Start_I2c(); /*对应数据通信示例的第一至第三周期*/ SendByte(0X20); SendByte(0X00); SendByte(0X00); Stop_I2c(); Delay1us(30000); //Delay1us(35000); } //-------------2.5us延时函数,stc12c5410ad主频24MHZ----------------- void Delay1us(unsigned int us)//改成2.5us以上的函数 { unsigned i,j; for(i=0;i<us;i++) { for(j=0;j<5;j++){;} //720对应2.5us,主频24MHZ } } //-------------------------串口定义--------------------------------- void RS232_init(void) { SCON=0x50;//0101 0000 PCON=0x00;//TMOD没有定义，main函数里已定义。 TH1=0xfd; TL1=0xfd; // 波特率 9600 TR1=1; } //-------------------------串口发送函数----------------------------- void serial_send(uchar dat) { SBUF=dat; while(TI==0); TI=0; } //-------------------------定时中断接受及处理数据-------------------- void timer0_receive(void) interrupt 1 { int i; uchar dispx[10]; TH0=0x00;//定时，与t的值相关 TL0=0x00; test=0;test1=1;Delay1us(1);//测试中断是否执行，中断时间是否超出TH0/TL0的设定值 Start_I2c(); SendByte(0X20); //对应数据通信示例的第四周期 SendByte(0X01); //第五周期 Start_I2c(); SendByte(0X21); //第六周期 for(i=0;i<3;i++) //第八至十周期（第七周期省略） { s[i]=RcvByte(); Ack_I2c(0); } s[3]=RcvByte(); //第十一周期 Ack_I2c(1); Stop_I2c(); ZuoBiao[0]=(s[0]<<8)+s[1];//十六位 ZuoBiao[1]=(s[2]<<8)+s[3]; //filter--------滤波函数，滤掉一些细小的变化，可有可无--------- iAccReal[0]=ZuoBiao[0]; if (abs(iAccReal[0x00]-iAccReal[0x01])<0x80) //0x80可变 { iAccFilter=iAccReal[0x00]; ZuoBiao[0]=iAccReal[0x01]; } else iAccReal[0x01]= iAccReal[0x00]; iAccReal[2]=ZuoBiao[1]; if (abs(iAccReal[0x02]-iAccReal[0x03])<0x80) { iAccFilter=iAccReal[0x02]; ZuoBiao[1]=iAccReal[0x02]; } else iAccReal[0x03]= iAccReal[0x02]; //----------------------------速度、位移算法----------------------- count++; if(count<=3) { ax[count]=(ZuoBiao[0]/512.0-4)*9.8; }//单位m/s2 else ax_temp=(ZuoBiao[0]/512.0-4)*9.8; if(count==3) { vx[1]=vx[0]+(ax[0]+4*ax[1]+ax[2])*t/6; vx[2]=vx[1]+(ax[1]+4*ax[2]+ax[3])*t/6; LX[1]=LX[0]+(vx[0]+4*vx[1]+vx[2])*t/6; } if(count>=4) { ax[1]=ax[2];ax[2]=ax[3];ax[3]=ax_temp; vx_temp=vx[2]+(ax[1]+4*ax[2]+ax[3])*t/6; vx[0]=vx[1];vx[1]=vx[2];vx[2]=vx_temp; LX[0]=LX[1]; LX[1]=LX[0]+(vx[0]+4*vx[1]+vx[2])*t/6; count=100; } //----------串口观察采集到的位移量-------------- dispx[0]='X'; dispx[1]=':'; if(LX[1]<0) dispx[2]='-'; else dispx[2]='+'; dispx[3]=(unsigned char)abs(LX[1]/10)+'0'; dispx[4]=(unsigned char)abs(LX[1])%10+'0'; dispx[5]='.'; dispx[6]=(unsigned char)abs(LX[1]*10)%10+'0'; dispx[7]=(unsigned char)abs(LX[1]*100)%10+'0'; dispx[8]=(unsigned char)abs(LX[1]*1000)%10+'0'; dispx[9]='m'; for(i=0;i<=9;i++) //串口显示调试 { serial_send(dispx[i]);Delay1us(500); } serial_send(' ');serial_send(' '); } //--------------------------------------------------------------------