% Simple Goalie Demo
% Copyright 2000  William Micheal Siever 

RobotComId=1;         % Use robot who's radio turret id is "1"
server='127.0.0.1';   % Use local machine as control server
BallId='Ball';        % Use the "Ball" to indicate the ball's center
RobotId='Blue';       % Use the "Blue" dot to indicate the robot's center
GoalId='Goal A';      % Aim for "Goal A"

disp('Setup:')
disp('    1) Vision Server (with "Blue" dot on robot 1)')
disp('    2) Robot 1 in front of Goal B facing North and wearing ID dot')
disp('       in center of body and running communications program')
disp('    3) Ball (on field)')
disp('    4) Communication Server (And radio base)')
disp('Press enter when ready');
pause;

% Only open the system once:
if exist('system_open')==0
    com_port=mComm('open',server);
    vis_port=mVision('open',server);
    system_open=1;
end

% Average Speed of Motion
avg_spd=20;

% Maximum number of pixels that should seperate the 
% Robot's center from the ball's center
max_dist=7;

% Get the goal's location
Goal=mVision('get',vis_port,GoalId);

% Infinite loop to control
while 1
    % Get the robot and ball's potision
    Robot=mVision('get',vis_port,RobotId);
    Ball=mVision('get',vis_port,BallId);
    
    if abs(Ball(2)-Robot(2)) > max_dist   
        % The Robot isn't close enough
  
        % If the ball is more north than the robot, move north
        % (as far as the goal's end)
        if Ball(2)<Robot(2) & Robot(2)>Goal(2)
            if mComm('set',com_port,RobotComId,avg_spd,avg_spd)~=0 
                disp('Error Setting Speed');
            end
        % If the ball is more south than the robot, move south
        % (as far as the goal's end)
        elseif Ball(2)>Robot(2) & Robot(2)<Goal(4)
            if mComm('set',com_port,RobotComId,-avg_spd,-avg_spd)~=0 
                disp('Error Setting Speed');
            end
        else
        % The robot is at one end of the goal - stop it.
        % (Keep it in the goal box)
            if mComm('set',com_port,RobotComId,0,0)~=0 
                disp('Error Setting Speed');
            end
        end
    else
        % The robot was close enough
        if mComm('set',com_port,RobotComId,0,0)~=0 
            disp('Error Setting Speed');
        end
    end
end
