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; } }