function monitor_PID(arg)

global
global
global
global

ejes plt status c_serie info

PUERTO BUFFER bytes_per_message Nshow
stream
K

% Objetos grficos
% Constantes

if nargin==0, arg='start'; end

switch arg
case 'start'
K = [0 0 0];
Nshow=300;
PUERTO='COM1'; BUFFER=32;
bytes_per_message=10; % Excluding A/Z, including packet counter
monitor_PID reset_stream;
monitor_PID crea_figura;
case 'reset_stream'
stream.buffersize=BUFFER;
stream.msg=[];
stream.status='busca_Z';
stream.fixed=bytes_per_message;
stream.data=uint8(zeros(1,stream.buffersize));
stream.res=[]; stream.n=0;
stream.busy=0;
stream.msg_read=0;
stream.bytes_read=0;
stream.last=0;
stream.com=[];
stream.log=-1;
case 'salir'
fclose all;
% Close any open files
obj=instrfind;
% Close serial objects
if ~isempty(obj),
if ~strcmp(obj.Status,'closed'), fclose(obj); end
delete(obj);
end
delete(gcf);
case 'crea_figura'
fig=figure; set(gcf,'Position',[100 100 400 500]);
set(gcf,'Number','off','Name','Monitor PID'); %'Menubar','none'
set(gcf,'Units','Norm','CloseRequestFcn','monitor_PID salir');
ejes=[];
ejes(1)=axes('Position',[0.07 0.77 0.9 0.18],'Box','on');
ejes(2)=axes('Position',[0.07 0.54 0.9 0.18],'Box','on');
ejes(3)=axes('Position',[0.07 0.30 0.9 0.18],'Box','on');
set(ejes,'FontSize',8);
plt=[];

set(gcf,'CurrentAxes',ejes(1));
plt(1)=plot([1:Nshow],zeros(1,Nshow),'b'); hold on
plt(2)=plot([1:Nshow],zeros(1,Nshow),'r');
% plot([1 Nshow],0*[1 1],'g:',[1 Nshow],500*[1 1],'g:');
hold off
title('Position');
set(ejes(1),'Ylim',[-150 650],'Xtick',[],'Ytick',[0:250:500]);
set(gcf,'CurrentAxes',ejes(2));
plt(3)=plot([1:Nshow],zeros(1,Nshow),'r'); hold on;
plot([1 Nshow],[0 0],'g:');
hold off
title('Speed');
set(ejes(2),'Ylim',[-50 50],'Xtick',[],'Ytick',[-50:25:50]);
set(gcf,'CurrentAxes',ejes(3));
plt(4)=plot([1:Nshow],zeros(1,Nshow),'k'); hold on
plot([1 Nshow],[512 512],'g:');
hold off
title('Duty cicle');
set(ejes(3),'Ylim',[-10 1050],'Xtick',[],'Ytick',[0:256:1024]);
DY=60; ALTO=80;
uicontrol('Style','frame','Position',[30 DY 100 ALTO]);
uicontrol('Style','frame','Position',[140 DY 240 ALTO]);
info=[]; label=[];
label(1)=uicontrol('Style','text','String','Pos :','Pos',[40 DY+55 40 18
]);
label(2)=uicontrol('Style','text','String','Err :','Pos',[40 DY+30 40 18
]);
label(3)=uicontrol('Style','text','String','Duty:','Pos',[40 DY+5 40 18]
);
set(label,'FontSize',12,'FontWeight','Bold','FontName','CourierNew');
for k=1:3,
pos = get(label(k),'Pos'); pos(1)=pos(1)+pos(3);
info(k) =uicontrol('Style','text','String','---','Pos',pos);
end
set(info,'FontSize',12,'FontWeight','Bold','FontName','CourierNew');
DX=150; DY=113; ANCHO=45;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Kp +','callback','monitor_PID send_orden','UserData'
,'7');
DX=DX+ANCHO+10;
%DY=DY-22;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Kp -','callback','monitor_PID send_orden','UserData'
,'4');
DX=DX+ANCHO+10;
%DY=DY-22;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Kp 0','callback','monitor_PID send_orden','UserData'
,'1');

%DX=DX+ANCHO+10; DY=113;
DY=DY-22;
DX=150;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Ki +','callback','monitor_PID send_orden','UserData'
,'8');
%DY=DY-22;
DX=DX+ANCHO+10;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Ki -','callback','monitor_PID send_orden','UserData'
,'5');
%DY=DY-22;
DX=DX+ANCHO+10;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Ki 0','callback','monitor_PID send_orden','UserData'
,'2');
%DX=DX+ANCHO+10; DY=113;
DX=150;
DY=DY-22;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Kd +','callback','monitor_PID send_orden','UserData'
,'9');
%DY=DY-22;
DX=DX+ANCHO+10;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Kd -','callback','monitor_PID send_orden','UserData'
,'6');
%DY=DY-22;
DX=DX+ANCHO+10;
uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 18],...
'String','Kd 0','callback','monitor_PID send_orden','UserData'
,'3');
DX = 320; DY=110;
for k=4:6,
info(k) =uicontrol('Style','text','String','---','Pos',[DX DY 55 20],.
..
'HorizontalAlig','Left');
DY=DY-22;
end
DY=30; ANCHO=75; DX=35;
c_serie(1)=uicontrol('Style','Edit','Position',[DX DY 42 20],...
'String',PUERTO,'HorizontalAlig','Left',...
'BackgroundCol',[0.9 1 0.9],...
'callback','monitor_PID set_port');
DX=90;
c_serie(2)=uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 20],..
.
'String','Open Serial','callback','monitor_PID open_close_seri
al');

DX=DX+ANCHO+15;
c_serie(3)=uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 20],..
.
'String','Start parser','callback','monitor_PID switch_procesa
');
DX=DX+ANCHO+15;
c_serie(4)=uicontrol('Style','Pushbutton','Position',[DX DY ANCHO 20],..
.
'String','Start log','callback','monitor_PID switch_log_on_off
');
DX=15;DY=7;
status=uicontrol('Style','Text','Position',[DX DY 380 18]);
set(status,'FontSize',10,'HorizontalAlig','Left');
set(status,'String','','BackgroundColor','White');
set(c_serie(3:end),'Enable','Off');
%DY=DY-20;

uicontrol('Style','frame','Position',[20 DY 770 4]

);
set(get(gcf,'Children'),'Units','Norm');
case 'send_orden'
ch=get(gcbo,'UserData');
fwrite(stream.com,ch,'uchar');
switch (ch)
case '7',
case '4',
case '1',
case '8',
case '5',
case '2',
case '9',
case '6',
case '3',
end

K(1)=K(1)+0.001;
K(1)=K(1)-0.001;
K(1)=0.0;
K(2)=K(2)+0.0005;
K(2)=K(2)-0.0005;
K(2)=0.0;
K(3)=K(3)+0.01;
K(3)=K(3)-0.01;
K(3)=0.0;

txt=sprintf('Kp=%.3f',K(1)); set(info(4),'String',txt);
txt=sprintf('Ki=%.3f',K(2)); set(info(5),'String',txt);
txt=sprintf('Kd=%.3f',K(3)); set(info(6),'String',txt);
%fprintf('Kp %.3f Ki %.3f Kd %.3f\n',K);
case 'set_port' % Cambia Puerto COM
s=get(gcbo,'String'); L=length(s); if L<4, return; end
if ~strcmp(s(1:3),'COM'), return; end
n = str2num(s(4:end)); if isempty(n), return, end
PUERTO=s;
case 'open_close_serial'
s = get(gcbo,'String');

if strcmp(s,'Open Serial'),
set(gcbo,'String','Close Serial');
monitor_PID connect_serial
else
set(gcbo,'String','Open Serial');
monitor_PID disconnect_serial
end
case 'connect_serial'
stream.parse=0;
stream.bytes_read=0;
stream.msg_read=0;
port=PUERTO;
set(status,'String',sprintf('Creating serial object'));drawnow;
stream.com=serial(port);
stream.com.BaudRate=115200; %57600; %38400 %115200; %9600; %115200;
stream.com.InputBufferSize=16*BUFFER;
stream.com.BytesAvailableFcnCount=BUFFER;
stream.com.BytesAvailableFcnMode='byte';
stream.com.BytesAvailableFcn='monitor_PID read_data';
try
set(status,'String',sprintf('Opening serial object %s',port)); drawnow
;
fopen(stream.com);
catch
fprintf('Problemas al abrir puerto serie\n');
delete(stream.com);
set(status,'String',sprintf('Failed to open port %s',port));
set(c_serie(2),'String','Open Serial');
return;
end
set(status,'String',sprintf('Connected to serial port %s',port));
set(c_serie(1),'Enable','off');
set(c_serie(3:end),'Enable','on');

case 'disconnect_serial'
fclose(stream.com); delete(stream.com);
set(status,'String','Puerto Serie Cerrado');
set(c_serie(1),'Enable','on');
set(c_serie(3:4),'Enable','off');

case 'read_data'
[data count]=fread(stream.com,BUFFER,'uchar');
if stream.log~=-1,
fwrite(stream.log,data(1:count),'uchar');
end;

if stream.parse==0,
stream.bytes_read=stream.bytes_read+count;
msg=sprintf(...
'Bytes sent/read %02d/%06d. ', ...
stream.com.ValuesSent,stream.bytes_read);
set(status,'String',msg);
return;
end

if ~stream.busy,
stream.busy=1;
stream.data=uint8(data);
parse_block;
N=stream.n; %fprintf('N msg %d ->\n',N);
% N = size(stream.res,2); fprintf('%d\n',N);
if N>0, % Refresca graficos e info
data = stream.res; % Resultados en stream.res
for k=[1 2 3 4]
new=data(k,:);
ydata=get(plt(k),'Ydata'); ydata=[ydata(N+1:end) new];
set(plt(k),'Ydata',ydata);
end
set(info(1),'String',sprintf('%5d',data(1,end)));
set(info(2),'String',sprintf('%5d',data(2,end)-data(1,end)));
set(info(3),'String',sprintf('%5d',data(4,end)));
end
stream.msg_read=stream.msg_read+N;
stream.n=0; stream.res=[];
stream.bytes_read=stream.bytes_read+count;
msg=sprintf(...
'Bytes sent/read %02d/%06d. MSG read %05d', ...
stream.com.ValuesSent,stream.bytes_read,stream.msg_read);
set(status,'String',msg);
stream.busy=0;
end
case 'switch_log_on_off'
if stream.log==-1, % Start log to a file
[fich dir]=uiputfile('.\*.pic');
if (fich~=0),
fich=[dir '\' fich];
stream.log=fopen(fich,'wb');
set(gco,'string','Stop Log');
end
else % Stop logger
fid=stream.log;

stream.log=-1;
fclose(fid);
set(gco,'string','Start Log');
end
case 'switch_procesa'
if stream.parse,
stream.parse=0; set(gco,'string','Start parser');
else
stream.parse=1; set(gco,'string','Stop parser');
end
end
function parse_block
global stream
ndata=length(stream.data);
for k=1:ndata,
c=char(stream.data(k));
switch stream.status
case 'busca_Z'
if c=='Z', stream.status='busca_A'; end
case 'busca_A'
if c=='A', stream.status='msg'; stream.msg=[];
% fprintf('Detectado ZA\n');
elseif c=='Z', stream.status='busca_A';
else stream.status='busca_Z';
end
case 'msg'
L=length(stream.msg);
if (L<stream.fixed), stream.msg=[stream.msg c];
else
if (c=='Z'), stream.status='end_msg';
else stream.status='busca_Z'; stream.msg=[];
end
end
case 'end_msg'
if c=='A', % Detectado ZA correcto of MSG
nb=length(stream.msg);
if nb~=stream.fixed,
% fprintf('BBAD msg -> %d bytes:: ',nb);
%
fprintf('%d ',stream.msg); fprintf('\n');
else
data=extrae_info(stream.msg);
stream.n=stream.n+1;
stream.res(:,stream.n)=data';
cont = stream.msg(end);
if ((stream.last+1 ~= cont) && (cont~=0)),
fprintf('Missing packet: %d --> %d\n', stream.last,cont);

end
stream.last=cont;
end
stream.msg=[];
stream.status='msg';
else % Falsa alarma,
stream.msg=[stream.msg c];
if c=='Z', stream.status='end_msg'; else stream.status='msg'; end
end
end
end
return

% End for k

function data=extrae_info(msg)
msg=uint8(msg);
pos = double(typecast(msg(1:4),'int32'));
vel = double(typecast(msg(5),'int8'));
pos_ref = double(typecast(msg(6:7),'int16'));
duty = double(typecast(msg(8:9),'uint16'));
data=[pos pos_ref vel duty];
return