private async void ImuEventHandler(object sender, ImuDataEventArgs e)
{
//Pitch/Roll correction
var newBodyRotZ = 0d;
var newBodyRotX = 0d;
if (e.Roll < 0)
e.Roll = e.Roll + 7; //7 is the error in degrees of the Roll calculation from the Razor
else
e.Roll = e.Roll - 7;
if (e.Roll < 0)
newBodyRotZ = Math.Round(e.Roll.Map(0, 25, 0, 10), 2);
else
newBodyRotZ = -Math.Round(e.Roll.Map(0, 25, 0, 10), 2);
if (newBodyRotZ < -10)
newBodyRotZ = -10;
if (newBodyRotZ > 10)
newBodyRotZ = 10;
if (e.Pitch < 0)
e.Pitch = e.Pitch + 3; //3 is the error in degrees of the Pitch calculation from the Razor
else
e.Pitch = e.Pitch - 3;
if (e.Pitch < 0)
newBodyRotX = Math.Round(e.Pitch.Map(0, 25, 0, 10), 2);
else
newBodyRotX = -Math.Round(e.Pitch.Map(0, 25, 0, 10), 2);
if (newBodyRotX < -10)
newBodyRotX = -10;
if (newBodyRotX > 10)
newBodyRotX = 10;
_bodyRotZ = newBodyRotZ;
_bodyRotX = newBodyRotX;
//*********************
//oscillation correction
if (_travelRequest)
_oscillations = 0;
if (e.AccelY < _restAccelY - 13 || e.AccelY > _restAccelY + 13)
_oscillations++;
if (_oscillateStopwatch.ElapsedMilliseconds > 10000)
{
_oscillations = 0;
_oscillateStopwatch.Restart();
}
if (_oscillateStopwatch.ElapsedMilliseconds <= 2000 || !(_oscillations > 9))
return;
_travelLengthZ = -1;
await Task.Delay(700).ContinueWith(t =>
{
_travelLengthZ = 0;
_oscillations = 0;
_oscillateStopwatch.Restart();
}).ConfigureAwait(false);
//*********************
}
