%%%**********************************************
%%%  Control Algorithm for Automated Bug Treadmill
%%%  ME-433 Advanced Mechatronics
%%%  Prof. Peshkin, Fall 2005
%%%  Erik Cruz and Owen Loh
%%%  Created: 10/15/05
%%%  Last updated: 12/04/05
%%%
%%% Usage:
%%% Program interfaces with two Yaskawa Servopack controlled motors via an
%%% SR626 A/D board. Object of the program is to maintain a bug on the top 
%%% of a sphere controlled with the Yaskawa motors.
%%%
%%% Note:
%%% This program uses the Image Acquisition Toolbox as well as a set of
%%% functions written by Dave Weir to communicate with the A/D board
%%%**********************************************



%% Once program is started, wait for external trigger to start control loop
dig_in = SR626_readDIO;
input = dec2bin(dig_in, 24);
while str2num(input(24)) < 1
    dig_in = SR626_readDIO;
    input = dec2bin(dig_in, 24);
    disp('Waiting for trigger. . .')
end

%%% begin control loop
disp('Starting Control. . .')
%% Start collecting images from camera
start(vidobj)

%% pause to esure first image is in buffer before control loop
%% and to increase speed of image acquistion
pause(1.5)

%% initialize previous offset variable for first control loop
offset_xp = 0;
offset_yp = 0;


while str2num(input(24)) == 1

    snapshot = peekdata(vidobj, 1);

    flushdata(vidobj)
    frames = vidobj.FramesAcquired;

    %% optional plotting commands (OPC) are included as comments for use in 
    %% debugging program operation
    %     imshow(snapshot) 
    %     hold on


    %% Find location of bug (offsets)
    %%%********************************************************

    %% turns image into black and white using threshold value
    %% a 1 is for white, 0 for black
    bw = im2bw(snapshot,.2); %%using .2 eliminates graythresh step
    
    % OPC
    % figure
    % imshow(bw)
    % hold on
    % impixelregion

    [rws,cols] = size(bw); %% finds the size of the image matrix in units of pixels

    blk_row = [];
    blk_col = [];
    blk = 1;
    for ii = 1:cols
        for jj = 1:rws
            if bw(jj,ii) == 0
                blk_row(blk) = jj;
                blk_col(blk) = ii;
                blk = blk + 1;
            end
        end
    end

    if isempty(blk_row) == 0

        %% finds the weighted average location of dark pixels
        %% ideally at the center of the mass to be localized
        [drk_row] = mean(blk_col);
        [drk_col] = mean(blk_row);

        center_row = cols/2;
        center_col = rws/2;


        offset_x = drk_row-center_row;
        offset_y = -(drk_col-center_col); %% negative sign since positive y is downward
        
        %%% basic speed control proportional to offset distance
%         speed1 = offset_x/400; %convert offset distance to voltage output
%         speed2 = -offset_y/200; %convert offset distance to voltage output
        
        
        %%% speed control method using previous position control
        % speed command is proportional to current position and previous
        % position
        speed1 = .8*(offset_x/400) +  .2*((offset_x/400) - (offset_xp/400));
        speed2 = .8*(offset_y/200) +  .2*((offset_y/200) - (offset_yp/200));
        
    else
        %%% when a bug is not found send a zero speed command
        speed1 = 0;
        speed2 = 0;

    end
    
    offset_x = offset_xp;
    offset_y = offset_yp;

    % OPC
    %plot(drk_row,drk_col,'r*','MarkerSize',10);


    %%% Send appropriate command signal
    %%%**********************************************************

    SR626_write4DAC([speed1 speed2  0 0]) %% writes speed voltage to channel 0


    while get(vidobj,'FramesAcquired') < frames + 1

    end

    %clear snapshot

    %% check digital input for trigger signal 
    dig_in = SR626_readDIO;
    input = dec2bin(dig_in, 24);
end

imshow(bw)
disp('Control Ended')
%% write zero speed signals when control is finished
SR626_write4DAC([0 0 0 0])
%% stop image acquisition
stop(vidobj)