mpu.dmpGetQuaternion(&q, fifoBuffer);//
mpu.dmpGetAccel(&r,fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal,&r,&gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
unsigned long nowtime =millis();
float acc1=aaWorld.x;
if(abs(acc1)<20)
{
AccX[0]=abs(acc1);
AccXT[0]=nowtime;
AccXT[2]+=nowtime;
/*if(AccXT[2]>1000)
{
AccXT[2]=0;
JL1=0;
}*/
}
if(abs(acc1)>20)
{
AccX[1]=abs(acc1);
AccXT[1]=nowtime;
JL=abs((AccX[0]*(AccXT[1]-AccXT[0])+0.5*AccX[1]*(AccXT[1]-AccXT[0])*(AccXT[1]-AccXT[0]))/1000000);//:s=v0t+at*t/2;
JL1=JL1+JL;
Serial.print("JL1\t");
Serial.print(JL1);
Serial.print("\t");
AccX[0]=AccX[1];
AccXT[0]=AccXT[1];
} |