function GoToPoint(x,y,min_err,vis_port,com_port,Kp,avg_spd,RobotId,Front,Back)
% Input:
%    x = X destination pixel
%    y = Y destination pixel
%    min_err = Minimum acceptable distance error in pixels
%    vis_port = Port to the vision server
%    com_port = Port to the communication server
%    Kp = Prportional constant
%    avg_spd = Robot average speed
%    RobotId = Radio Turret Id of robot to control
%    Front = Text Label of a dot on the robot's front
%    Back = Text Label of a dot on the robot's back
% Output:
%    The robot is moved to within min_err pixels of the destination
%% 
%% Simple Go To Point using prportinoal control
%% Copyright 2000  William Micheal Siever 

% Get the robot's current position
RobotFront=mVision('get',vis_port,Front);
RobotBack=mVision('get',vis_port,Back);
Rx=(RobotFront(1)+RobotFront(3)/2 + RobotBack(1)+RobotBack(3)/2)/2;
Ry=(RobotFront(2)+RobotFront(4)/2 + RobotBack(2)+RobotBack(4)/2)/2;

% Determine the error
err=sqrt((Rx-x)^2+(Ry-y)^2);


while err>min_err

    % Calculate Rt (Angle to destination)
    Rt=[RobotFront(1)-RobotBack(1) RobotFront(2)-RobotBack(2)];
    Rt=Rt/sqrt(Rt*Rt');

    % Compute the angle in [0,2*pi]
    if Rt(1)>0     % 1st or 2nd quadrant
        Rt=  pi/2+asin(Rt(2));
    else           % 3rd or 4th quadrant
        Rt=3*pi/2-asin(Rt(2));
    end
    
    % Determine the angle between the robot and the destination
    delta_t=DeltaAngle(Rx,Ry,Rt,x,y);
    
    % Compute the instant error
    err=0-delta_t;    

    % Generate new motor values based on PID control
    delta=abs((Kp*err)/(2*pi)*avg_spd);
    if delta_t>0
        lm=avg_spd-delta;
        rm=avg_spd+delta;
    elseif delta_t<0
        lm=avg_spd+delta;
        rm=avg_spd-delta;
    else
        lm=avg_spd;
        rm=avg_spd;
    end
    
    % motor velocities have to be integers
    lm=floor(lm);
    rm=floor(rm);

    if mComm('set',com_port,RobotId,lm,rm)~=0 
        disp('Error Setting Speed');
    end

    % Get the robot's current position
    RobotFront=mVision('get',vis_port,Front);
    RobotBack=mVision('get',vis_port,Back);
    Rx=(RobotFront(1)+RobotFront(3)/2 + RobotBack(1)+RobotBack(3)/2)/2;
    Ry=(RobotFront(2)+RobotFront(4)/2 + RobotBack(2)+RobotBack(4)/2)/2;    

    err=sqrt((Rx-x)^2+(Ry-y)^2);
end

if mComm('set',com_port,RobotId,0,0)~=0 
    disp('Error Setting Speed');
end
