public class PlannerEx2 extends Planner {

	boolean firstPose;

	// Constructor for the planner
	public PlannerEx2() {
		firstPose = true;
	}

	// Called when the planner receives a pose from the tracker
	public void receivedPoseFromTracker(Pose p) {
		if (firstPose) {
			byte[] outData = new byte[6];
			outData[0] = (byte)(p.x / 256);
			outData[1] = (byte)(p.x % 256);
			outData[2] = (byte)(p.y / 256);
			outData[3] = (byte)(p.y % 256);
			outData[4] = (byte)(p.angle / 256);
			outData[5] = (byte)(p.angle % 256);
			sendDataToRobot(outData);
			firstPose = false;
		}
	}

	// Called when the planner receives data from the robot
	public void receivedDataFromRobot(int[] data) {
		int x = data[0]*256 + data[1];
		int y = data[2]*256 + data[3];
    	int a = data[4]*256 + data[5];
    	sendEstimatedPositionToTracker(x, y, a);
	}

	// Called when the planner receives data from another station
	public void receivedDataFromStation(int stationId, int[] data) {
	}
}