%Scott Mueller
%3/10/09
%This is the Servo Calibration Routine for the vision controlled nerf gun
%Remove all targets from black background prior to running
%Be sure the laser pointer on the pan/tilt assembly is turned on
%This routine will step the servos across the board, finding the pixel
%position of the laser each time and creating a lookup table of servo
%positions versus pixel positions 
%The user must manually determine the bottomLeft and topRight servo
%position parameters indicated below.  Call panTilt(tilt,pan) directly from
%the console to move the laser to the approximate lower left and top right
%corner of the black background.  These positions must lie within the ROI
%drawn so it is better to err towards the inside of the
%image

function pwmToPixel = servoCalibration()

    %////////MODIFY THESE PARAMETERS///////////////////////////////////////////
    bottomLeft=round([2750,2250]); %these are the (tilt,pan) servo positions for the bottom left corner of the target area
    topRight =round([3000,2000]); %these are the (tilt,pan) servo positions for the top right corner of the target area
    %//////////////////////////////////////////////////////////////////////////
    increments = 10;  %number of steps to take to move through image
    inc=(topRight-bottomLeft)./increments ; %amount to move for eachs tep

    %creates table of servo positions that move from the bottomLeft to topRight
    %in increments # of steps
    panTiltMotion = round([bottomLeft(1):inc(1):topRight(1);bottomLeft(2):inc(2):topRight(2)]);

    vidobj = videoinput('winvideo'); %setup video object
    Im = getsnapshot(vidobj); %get frame


    temp = size(Im)
    figure, imshow(Im);

    %////////FINDS BOUNDS OF BLACK BACKGROUND IN IMAGE/////////////////////////
    %creates a binary matrix with ROI set to 1
    h = imrect; %select ROI: user must use mouse to draw rectangle selecting part of image to be processed:  select only black background
    position = wait(h)
    position=round(position);
    Im(1:position(2),:,:)=0; %top boundary = 0
    Im(position(2)+position(4):temp(1,1),:,:)=0; %bottom boundary = 0
    Im(:,1:position(1),:)=0; %left boundary = 0
    Im(:,position(1)+position(3):temp(1,2),:)=0; %right boundary = 0
    %///////////////////////////////////////////////////////////////////////////

    greenThresh = 200;  %greeness threshold for extracting laser

    boundaryMask = Im(:,:,2)>0;
    Im = Im(:,:,2) > greenThresh; %applies a threshold to green portion of image to find laser
    imshow(Im); %display

    pwm = 1500;
    pwmToPixel = ones(2,increments);
    tempSize = size(panTiltMotion);
    tempSize(1,2)

    panTilt(bottomLeft(1),bottomLeft(2));  %move servo to bottom left prior to starting routine
    pause(3); %give it time to get there
        for i=1:tempSize(1,2)
            i
         %*****UNCOMMENT THIS LINE******   
            panTilt(panTiltMotion(1,i),panTiltMotion(2,i));  %drives servos to new position in motion table
            pause(2);  %LET THE servo move to position
          %*****UNCOMMENT THIS LINE******   

            Im = getsnapshot(vidobj);
            imshow(Im);

            ImBoundLowerLeft = [15;161];
            ImBoundUpperRight = [312;36];  %(x,y)
            temp = size(Im);

            Im(1:position(2),:,:)=0; %top
            Im(position(2)+position(4):temp(1,1),:,:)=0; %bottom
            Im(:,1:position(1),:)=0; %left
            Im(:,position(1)+position(3):temp(1,2),:)=0; %right 
            Im = Im(:,:,2) > greenThresh;
            imshow(Im);

            ImB = Im;
            imshow(ImB);
            %pause(10);
            %ImB=bwareaopen(ImB,5);  %remove small objects

            ImL=bwlabel(ImB);
            imshow(ImL);
            stats = regionprops(ImL,'all');
            centroid=stats(1).Centroid;

            pwmToPixel(1,i)=panTiltMotion(1,i);
            pwmToPixel(2,i)=centroid(1,2);
            pwmToPixel(3,i)=panTiltMotion(2,i);
            pwmToPixel(4,i)=centroid(1,1);
            %pwmToPixel(:,i)=[5;6;7;8];%[panTiltMotion(1,i),centroid(1,2),panTiltMotion(2,i),centroid(1,1)]  %[tilt pwm; tilt pixel; pan pwm; pan pixel]


            hold on
            plot(centroid(1,1),centroid(1,2),'*b');
            hold off

        end
end
