private void HandleMonitor4(BinaryReader br, double ts)
{
RMPMonitor4Msg msg = new RMPMonitor4Msg(br);
if (isBackwards)
{
status.integratedLeftWheelPosition = -msg.integratedRightWheelDisp * 2.0 / 40181.0;
status.integratedRightWheelPosition = -msg.integratedLeftWheelDisp * 2.0 / 40181.0;
status.integratedLeftWheelPositionTicks = -msg.integratedLeftWheelDisp;
status.integratedRightWheelPositionTicks = -msg.integratedRightWheelDisp;
}
else
{
status.integratedLeftWheelPosition = msg.integratedLeftWheelDisp * 2.0 / 40181.0;
status.integratedRightWheelPosition = msg.integratedRightWheelDisp * 2.0 / 40181.0;
status.integratedLeftWheelPositionTicks = msg.integratedLeftWheelDisp;
status.integratedRightWheelPositionTicks = msg.integratedRightWheelDisp;
}
status.wheelPosTs = ts;
wheelPosTs = ts;
if (WheelPositionUpdate != null)
{
WheelPositionUpdate(this, new TimestampedEventArgs <IRobotTwoWheelStatus>(ts, new SegwayStatus(status)));
}
}
private void HandleMonitor4(BinaryReader br, double ts)
{
RMPMonitor4Msg msg = new RMPMonitor4Msg(br);
if (isBackwards)
{
status.integratedLeftWheelPosition = -msg.integratedRightWheelDisp * 2.0 / 40181.0;
status.integratedRightWheelPosition = -msg.integratedLeftWheelDisp * 2.0 / 40181.0;
}
else
{
status.integratedLeftWheelPosition = msg.integratedLeftWheelDisp * 2.0 / 40181.0;
status.integratedRightWheelPosition = msg.integratedRightWheelDisp * 2.0 / 40181.0;
}
status.wheelPosTs = ts;
wheelPosTs = ts;
if (WheelPositionUpdate != null) WheelPositionUpdate(this, new TimestampedEventArgs<IRobotTwoWheelStatus>(ts, new SegwayStatus(status)));
}
