% Copyright (c) 2010 ARM Ltd
%  
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
%  
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%  
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.

MaxRange = 80;
MaxServoAngle = 150;
READINGS = 35;
NoScans = 10;
Angles = zeros((READINGS+1), 1);
Range = zeros((READINGS+1), 1);

%Set up communication to mbed
mbed = SerialRPC('COM7', 9600);
mbed.reset();
pause(0.5);
theta_Servo = PwmOut(mbed, p21);
phi_Servo = PwmOut(mbed, p22);
StopButton = DigitalIn(mbed, p5);
srf08 = RPCFunction(mbed,'RangeFinder');

theta_Servo.period(0.020);
phi_Servo.period(0.020);

%set servos to start position and give them time to get there
theta_Servo.pulsewidth(0.002);
phi_Servo.pulsewidth(0.001);
pause(0.5)

for i = 1:1:NoScans;
    hold off;
    n = 1;
    p = 1;
    plotPoints = zeros((READINGS+1), 2);
    phi_Servo.pulsewidth(1.2/1000);
    for t = 1:(1/READINGS):2;  
        
        theta_Servo.pulsewidth((3 - t)/1000);
        %give the servo time to get into position
        pause(0.001);
        %Read the distance into the Range matrix 
        Range(n) = str2double(srf08.run(' ')) + 3;
        if Range(n) < 4
           Range(n) = 0;
        end
        if Range(n) > MaxRange
           Range(n) = 0;
        end
        %Calculate the angles column 1 is theta, 2 is phi     
        Angles(n) = (MaxServoAngle/2) * ((t-1.5) * 2); 
        %Add the points which are valid to the maxtrix
        if Range(n) > 0
           plotPoints(p,1) = pi .* Angles(n) ./ 180 + pi/2;
           plotPoints(p,2) = Range(n);
           p = p + 1;
        end
        n = n + 1; 
        %plot the hits
        polar(plotPoints(:,1), plotPoints(:,2), 'r-');
        
        if StopButton.read() == 0
            display('Stop button pressed');
        break;  
        end
    end
    if StopButton.read() == 0
        display('Stop button pressed');
    break; 
    end
    pause(0.01);
    disp('Completed a scan');
end   %repeat the scan a number of times
display('Completed RangeFinder demo');
mbed.delete
clear