clc
close all
a = arduino();
s = servo(a, 'D4', 'MinPulseDuration', 1000*10^-6, 'MaxPulseDuration', 2000*10^-6); 
cam = webcam('USB Camera'); 
%% Servomotor zapojen na digitální pin è. 4
laser = 100; %% citlivost lokalizace laserové èáry (0;255)
horni = 0.45;  %% horní pozice servomotoru
dolni = 0.9; %% dolní pozice servomotoru 

writePosition(s,horni);          
rgbim = snapshot(cam); %% snímek v RGB
clear rows columns D A sirka vyska OUTPUT sloupce radky H Vzdalenost x
IMG = rgb2gray(rgbim); %% pøevedení RGB snímku na èernobílý

[rows, columns] = find(IMG > laser); %% lokalizace èáry - vytvoøení dvou vektorù
x = numel(rows);

K(1:x) = 610.72;
D = rows.^3*0.1446 - 7.0104*rows.^2 + 115;12*rows - K';

%% Výpoèet výšky a šíøky bodu ze vzdálenosti
 %% Kamera má pøibližné stejnì široké i vysoké zorné pole, kde jeho velikost
 %% odpovídá i vzdálenosti clonky od kamery
Vzdalenost = D;
vyska (1:x) = 480;
radky = (Vzdalenost./vyska').*(240-rows);

sirka (1:x) = 640;
sloupce = (Vzdalenost./sirka').*(columns-320);

OUTPUT = horzcat(radky, sloupce, Vzdalenost); 
TF = OUTPUT(:,3) < 10 | OUTPUT(:,3) > 130; %% kontrola aktivního rozsahu
OUTPUT(TF,:)=[];
%% levý sloupec - výška bodu od osy kamery
%% støední sloupec - šíøka bodu od osy kamery
%% pravý sloupec - vzdálenost bodu od èela kamery

% plot3(OUTPUT(:,2),OUTPUT(:,3),OUTPUT(:,1),'+') %% mapa bodù
% grid on


