%% A5 WaypointNav
% This script starts a robot and drives it in a closed-loop fashion to each
% of a list of goals.

start = [3 0];			%initial position
goals = [5 5;0 0]';		%column-vector list of goals

gI = 1;					%denoted the index of the current goal in list goals


%% spawn robot and start main loop
rob = initializeRobot('rob','P2AT',[start 1.8],[0 0 0]);
pause(2);

INS = getINSReadings(rob);
pos = INS.Position(1:2);
orient = INS.Orientation(3);

%while we are not at the last goal or we are not heading to the last goal
while(norm(goals(:,end) - pos) > .5 || gI ~= size(goals,2))
	
	%increment goal index if we hit this one
	if(norm(pos - goals(:,gI)) < .5)
		gI = gI + 1;
	end
	
	% Calculate motorCommand
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	%Your code here
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	% Send the command to the robot
	sendDriveCommand(rob,motorCommand);	
	
	%pause a bit
	pause(.05);
	
	% Update  position information.
	INS = getINSReadings(rob);
	pos = INS.Position(1:2);
	orient = INS.Orientation(3);
end

%We're there! Shut down the robot.
shutdownRobot(rob);

