%% PROPORTIONAL CONTROLLER 

%%
% Compute error
xErrLin = coord(1) - centers(1,1); 
yErrLin = coord(2) - centers(1,2); 

% Compute control signal
posPan = posPan + kPan*xErrLin;
posTilt = posTilt + kTilt*yErrLin;

% Upper and lower limit
posPan = min(0.99, max(0.01, posPan));
posTilt = min(0.99, max(0.01, posTilt));

% Print for debug
% posPan
% posTilt

% Send control to servos
writePosition(sPan, posPan);
writePosition(sTilt, posTilt);

pause(0.01);