void robot_WheelSpeedUpdate(object sender, Magic.Common.TimestampedEventArgs <IRobotTwoWheelStatus> e)
{
lastVelTS = velTS;
lastRWheelVel = rWheelVel;
lastLWheelVel = lWheelVel;
rWheelVel = e.Message.RightWheelSpeed;
lWheelVel = e.Message.LeftWheelSpeed;
velTS = e.TimeStamp;
if ((velTS - lastVelTS) > 0)
{
rWheelAccel = (rWheelVel - lastRWheelVel) / (velTS - lastVelTS);
lWheelAccel = (lWheelVel - lastLWheelVel) / (velTS - lastVelTS);
}
else
{
rWheelAccel = 0;
lWheelAccel = 0;
}
}
void robot_WheelPositionUpdate(object sender, Magic.Common.TimestampedEventArgs <IRobotTwoWheelStatus> e)
{
lWheelPos = e.Message.IntegratedLeftWheelPosition;
rWheelPos = e.Message.IntegratedRightWheelPosition;
}