public static void ProcessPathPackets()
{
for (int i = 0; !PathPackets.IsEmpty() && i < NUM_PACKETS_TO_PROCESS; i++)
{
Packet p = PathPackets.Dequeue();
switch ((PacketID)p.Data.ID)
{
// TODO Maybe: Combine pathing speed and turn in same packet???
case PacketID.PathingSpeed:
PathSpeed = UtilData.ToFloat(p.Data.Payload);
MotorControl.SkidSteerDriveSpeed(PathSpeed, PathAngle);
break;
case PacketID.PathingTurnAngle:
PathAngle = UtilData.ToFloat(p.Data.Payload);
MotorControl.SkidSteerDriveSpeed(PathSpeed, PathAngle);
break;
}
}
}
