%% Dynamixel RX-64 - hlavni program
% Martin Zeman, 2009

clear all;
close all;
clc;

% digitalizační zdroj
img_src_info    = imaqhwinfo('winvideo');
img_device_info = img_src_info.DeviceInfo(1);

% video tok a nastavení kamery
kamera = videoinput('winvideo',1);
src = getselectedsource(kamera);
src.saturation = 0;
set(kamera,'ReturnedColorSpace','grayscale');
triggerconfig(kamera, 'manual');
start(kamera);

%% Seriove linky
s=serial('COM4','BaudRate',57600);
fopen(s);

c = serial('COM1');
fopen(c);

%% Inicializace kamery
string=sprintf('lfp0300'); %ostreni (rozsah: lfp0000 ~ lfp0400)
fprintf(c, '\002x%s\003x', string);
string=sprintf('lap0050');  %clona (rozsah: lap0000 ~ lap0400)
fprintf(c, '\002x%s\003x', string);
string=sprintf('sspa0');  %zoom (rozsah: ssp00 ~ sspff)
fprintf(c, '\002x%s\003x', string);
pause(1);

%% Prikazy
lr = 290;
ud = 510;
send(s,leftright(lr));
send(s,updown(ud));
pause(3);
posun = 5;
for i = 1:20
    obraz = getsnapshot(kamera);
    siz = size(obraz);
    obraz = obraz(80:siz(1),:);
    nazev = sprintf('obraz%d.jpg',i) %#ok
    imwrite(obraz,nazev);
    [sX sY ok] = detekce(obraz) %#ok
    dim = size(obraz);
    if ok == 0
        break;
    end
    pause(5);
    if (sX>(dim(2)/2+50)) && (lr>250) %doprava
        lr = lr - posun;
        send(s,leftright(lr));
    elseif (sX<(dim(2)/2-50)) && (lr<350) %doleva
        lr = lr + posun;
        send(s,leftright(lr));
    end

    if (sY>(dim(1)/2+50)) && (ud<550) %dolu
        ud = ud + posun;
        send(s,updown(ud));
    elseif (sY<(dim(1)/2-50)) && (ud>450) %hore
        ud = ud - posun;
        send(s,updown(ud));
    end
    pause(0.5);
end

%% Ekvalizace histogramu
imshow(histogram(obraz));

%% Konec
fclose(s);
fclose(c);
stop(kamera);
delete(kamera);
