internal void update(DataHandler toUpdate)
{
TBVelocity frame = new TBVelocity();
frame.Id = Constants.TB_VELOCITY_ID;
this.check();
if (velocityLeft > 0 && velocityRight > 0)
{
frame.SubId = Constants.TB_VELOCITY_FORWARD;
}
else if (velocityLeft > 0 && velocityRight < 0)
{
frame.SubId = Constants.TB_VELOCITY_TURN_RIGHT;
}
else if (velocityLeft < 0 && velocityRight < 0)
{
frame.SubId = Constants.TB_VELOCITY_BACKWARD;
}
else if (velocityLeft < 0 && velocityRight > 0)
{
frame.SubId = Constants.TB_VELOCITY_TURN_LEFT;
}
frame.TimeStamp = 0;
frame.speedLeft = (byte)Math.Abs(velocityLeft);
frame.speedRight = (byte)Math.Abs(velocityRight);
toUpdate.update(frame);
}
private void _receiveMessage()
{
switch (_Frame.Id)
{
case Constants.TB_COMMAND_ID:
break;
case Constants.TB_VELOCITY_ID:
if (!(_Frame is TBVelocity))
{
throw new Exception("Frame is not a Velocity Frame");
}
TBVelocity velocityData = (TBVelocity)_Frame;
if (velocityData.SubId == Constants.TB_VELOCITY_FORWARD)
{
_Bot.setVelocity(velocityData.speedLeft * 4, velocityData.speedRight * 4, WheelDirection.Forwards, WheelDirection.Forwards);
}
else if (velocityData.SubId == Constants.TB_VELOCITY_BACKWARD)
{
_Bot.setVelocity(velocityData.speedLeft * 4, velocityData.speedRight * 4, WheelDirection.Backwards, WheelDirection.Backwards);
}
else if (velocityData.SubId == Constants.TB_VELOCITY_TURN_LEFT)
{
_Bot.setVelocity(velocityData.speedLeft * 4, velocityData.speedRight * 4, WheelDirection.Backwards, WheelDirection.Forwards);
}
else if (velocityData.SubId == Constants.TB_VELOCITY_TURN_RIGHT)
{
_Bot.setVelocity(velocityData.speedLeft * 4, velocityData.speedRight * 4, WheelDirection.Forwards, WheelDirection.Backwards);
}
break;
case Constants.TB_POSITION_ID:
if (!(_Frame is TBPosition))
{
throw new Exception("Frame is not a Position Frame");
}
TBPosition positionData = (TBPosition)_Frame;
_Bot.PositionRequested = true;
if (positionData.SubId == Constants.TB_POSITION_FORWARD)
{
_Bot.setPosition((ushort)positionData.distance);
}
else if (positionData.SubId == Constants.TB_POSITION_BACKWARD)
{
_Bot.setPosition(-(ushort)positionData.distance);
}
else if (positionData.SubId == Constants.TB_POSITION_TURN_LEFT)
{
_Bot.setAngle(-(ushort)positionData.distance);
}
else if (positionData.SubId == Constants.TB_POSITION_TURN_RIGHT)
{
_Bot.setAngle((ushort)positionData.distance);
}
break;
case Constants.TB_ERROR_ID:
break;
}
}
