%% Key_WaypointPotField
% 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

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

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))
	
	if(norm(pos - goals(:,gI)) < .5)
		gI = gI + 1;
	end
	
	%desired heading
	desvH = Key_PotField_user(pos,goals(:,gI));
	desvH = desvH / norm(desvH);
	
	% actual heading
	vH = [cos(orient);sin(orient)];
	
	% Calculate forward and rotational velocity.
	omega = turnConst*desvH'*[0 -1;1 0]*vH;				% cross product
	V = moveConst*desvH'*vH * (abs(desvH'*vH) > .7);	% dot product
	
	% 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);

