%% WaypointPotField_user
% This script starts a robot and drives it in a closed-loop fashion to each
% of a list of goals. This code should be used with the CircularWorkspace
% provided in the homework files. The locations of obstcles are given, and
% the start/goal points given are not necessarily the ones used for
% grading.

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

turnConst = 80;			%speed constant for rotational motion
moveConst = 40;			%speed constant for linear motion

gI = 1;					%The gI variable denotes the current goal index 
						%in the list of indeces. ie if you're moving toward
						%goal 1, gI = 1 and the goal is goals(:,gI)

%% 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( %%% WHILE WHAT? %%%)
	
	%%%%%%% Have we reached our current goal? Check and see. If so,
	%%%%%%% increment the goal index.
	
	%%%%%%% Find desired heading to compute wheel speeds.
	
	% Calculate forward and rotational velocity (in percent).
	omega = %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	V = %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	
	% Send the command to the robot
	sendDriveCommand(rob,V+[omega,-omega]);	
	
	%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);

