/*!
@brief Calculates the speed of hovercraft
@param st_speed is the pointer returned from create_speed(void)
*/
void calculate_speed(Speed* st_speed){
unsigned long deltaTime; /**< subtraction of start and end time */
unsigned long time;
// Get current time of board
time = millis();
deltaTime = time - st_speed->time;
st_speed->speed += (getXAccel(st_speed->bias) * deltaTime)/1000;
st_speed->time = time;
}
/// getOrientation returns which axis perpendicular with the earths surface x=1,y=2,z=3 is positive or
/// negative depending on which side of the axis is pointing downwards
int AcceleroMMA7361::getOrientation()
{
int gemiddelde = 10;
int x = 0;
int y = 0;
int z = 0;
int xAbs = 0;
int yAbs = 0;
int zAbs = 0;
for(int i = 0; i<gemiddelde ; i++) //We take in this case 10 measurements to average the error a little bit
{
x = x+getXAccel();
y = y+getYAccel();
z = z+getZAccel();
}
x= x/gemiddelde;
y = y/gemiddelde;
z = z/gemiddelde;
xAbs = abs(100-abs(x));
yAbs = abs(100-abs(y));
zAbs = abs(100-abs(z));
if (xAbs<yAbs&&xAbs<zAbs)
{
if (x>0)
{
return 1;
}
return -1;
}
if (yAbs<xAbs&&yAbs<zAbs)
{
if (y>0)
{
return 2;
}
return -2;
}
if (zAbs<xAbs&&zAbs<yAbs)
{
if (z>0)
{
return 3;
}
return -3;
}
return 0;
}
