% Simple Kicker Demo
% Copyright 2000  William Micheal Siever 

RobotId=2;             % Use robot who's radio turret id is "2"
server='127.0.0.1';    % Use local machine as control server
BallId='Ball';         % Use the "Ball" to indicate the ball's center
GoalId='Goal A';       % Aim for "Goal A"
RobotFrontId='KFront'; % Use the "KFront" dot to indicate the front of robot 2
RobotBackId='KBack';   % Use the "KBack" dot to indicate the back of robot 2

disp('Setup:')
disp('    1) Vision Server (with "KFront" dot on the front of robot 2')
disp('       and "KBack" dot on back of robot 2');
disp('    2) Robot 2 facing the ball which faces toward Goal B, ')
disp('       wearing ID dots, and running communications program')
disp('    3) Ball (on field between robot 2 and Goal B)')
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

% Find the Center of the Goal
Goal=mVision('get',vis_port,GoalId);
GoalX=(Goal(1)+Goal(3))/2;
GoalY=(Goal(2)+Goal(4))/2;

% Get the ball's current position
tmp=mVision('get',vis_port,BallId);

% The ball is about 4.26 cm in diameter - compute the
% pixel to cm conversion factor
cm=tmp(3)/4.26;

% Distance to go behind the ball to kick and to go "through" the ball
punt_dist=20*cm;

% Speed to use to change orientation
orient_spd=10;
% Acceptable Orientation Error in radians
orient_err=0.7;

% Acceptable Position Error
min_err=10;
% Average Foreward Velocity
avg_spd=25;
% Proportional Constant
Kp=3.5; % This is dependent on the speed of the machine running the prog.

% Infinite loop to control
while 1
    % Get the ball's current position
    Ball=mVision('get',vis_port,BallId);

    % 1) Find the Start Point and the Kick Point
    if GoalX==Ball(2)  % See if the ball is lined up with the goal
        SpX=Ball(1);
        SpY=Ball(2)+punt_dist;
        KpX=Ball(2);
        KpY=Ball(2)-punt_dist;
    else
        m=(GoalY-Ball(2))/(GoalX-Ball(1));
        b=Ball(2)-Ball(1)*m;
        SpX=Ball(1)+punt_dist;
        SpY=m*SpX+b;
        KpX=Ball(1)-punt_dist;
        KpY=m*KpX+b;
    end

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

    % 2) Go to the Start Point
    GoToPoint(SpX,SpY,min_err,vis_port,com_port,Kp,avg_spd,...
              RobotId,RobotFrontId,RobotBackId);

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

    % 3) Orient Towards the Ball
    Orient(Ball(1),Ball(2),orient_err,vis_port,com_port, ...
           orient_spd,RobotId,RobotFrontId,RobotBackId);

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

    % 4) Go to the Kick Point
    GoToPoint(KpX,KpY,min_err,vis_port,com_port,Kp,avg_spd,...
              RobotId,RobotFrontId, RobotBackId);

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

    disp('Done - Setup robot and press enter to run again');
    pause;
end
