% Control the PTU and take distance readings
% 11 April 2006     MapBot Project    Stephanie Luft
% This program communicates with the PTU server to control its movements
% and take distance readings every 5 degrees.  It requires that the
% tcp_udp_ip directory to be placed in the same folder as the directory
% holding runPTU.m


% Add PTU path
addpath('..\tcp_udp_ip')

% Open PTU connection
connection=pnet('tcpconnect','localhost',8082);
if connection==-1
    disp('ERROR: CONNECTION TO PTU NOT MADE')
    return      %break program after error
end   
str=pnet(connection,'readline');

% Calibrate PTU 
pnet(connection,'printf','PP0\n')
str=pnet(connection,'readline');
if str(1)~= '+'
    disp('ERROR: INVALID COMMAND OR OTHER PROBLEM WITH PTU')
    disp(str)
end
pnet(connection,'printf','TP0\n')
str=pnet(connection,'readline');
if str(1)~= '+'
    disp('ERROR: INVALID COMMAND OR OTHER PROBLEM WITH PTU')
    disp(str)
end

% Move PTU Number of Positions to Phi (-160 to +160 degrees)
phistep=-160:5:160;
[m,n]=size(phistep);
for i=1:n
    position=round(phistep(i)*19.4445);     % step 5 degrees
    if position>3090
        position=3090;
    elseif position<-3089
        position=-3089;
    end
    pos=int2str(position);
    string=strcat('PP',pos,'\n');
    pnet(connection,'printf',string)
    str=pnet(connection,'readline');
    if str(1)~= '+'
        disp('ERROR: INVALID COMMAND OR OTHER PROBLEM WITH PTU')
        disp(str)
    end
    
% Run ImageCapture
   sim('ImageCapture')
    pic=red(:,:,2)+green(:,:,2)+blue(:,:,2);     
        % Have to use 2nd set of data because 1st tends to come out black
        
% Calculate Distance to Obstacle
    dist=finddistance(pic);

% Record Distance and Phi
    distance(i)=dist;
    if phistep(i)<=0
        phistep(i)=360-abs(phistep(i));
    end
    phi(i)=phistep(i);
end

% Reset PTU 
pnet(connection,'printf','PP0\n')
str=pnet(connection,'readline');
if str(1)~= '+'
    disp('ERROR: INVALID COMMAND OR OTHER PROBLEM WITH PTU')
    disp(str)
end
pnet(connection,'printf','TP0\n')
str=pnet(connection,'readline');
if str(1)~= '+'
    disp('ERROR: INVALID COMMAND OR OTHER PROBLEM WITH PTU')
    disp(str)
end

% Close connection
pnet(connection,'close')