cnf_set.m

hard disk drive servo control tools, it is very helpful

    set(handles.edit_OV_gain, 'String', cnfdb.Entry.K );
end    
set(handles.edit_OV_epsilon,'String',matrix2str(cnfdb.Controller.OV_poles(1)));
set(handles.edit_OV_omega,'String',matrix2str(cnfdb.Controller.OV_poles(2)));

% Filter
set(handles.check_FT,'Value', cnfdb.Controller.FT_in);
% [N1 D1]=tfdata(cnfdb.Controller.FT_Fcn, 'v');
% set(handles.edit_FT_Fcn,'String', strcat('tf(', matrix2str(N1), ',', matrix2str(D1), ')' ) );
set(handles.edit_FT_Fcn,'String',cnfdb.Controller.FT_Fcn);
if cnfdb.Controller.FT_in==0
    set(get(handles.check_FT,'UserData'), 'Visible', 'off');
end


% Nonlinearity compensation
set(handles.check_NL, 'Value', cnfdb.Controller.NC_in);
if cnfdb.Controller.NC_in==0
    set(get(handles.check_NL,'UserData'), 'Visible', 'off');
end
set(handles.edit_NF, 'String', cnfdb.Controller.NC_Fcn);

% Make figure visible and store database
set(ud.Figure,'UserData', ud, 'Visible','on');
update_controller_diagram;



% update_controller_diagram
% --------------------------------------------------------
function update_controller_diagram(varargin)
ud=get(gcf,'UserData');
handles=ud.Handles;
gdb=[get(handles.check_IC,'Value'), get(handles.check_CNF,'Value'), get(handles.check_NL,'Value'), get(handles.radio_OV_none,'Value'), get(handles.check_FT,'Value') ];
set(ud.DiagramHandle,'UserData',gdb);
cnf_diagram(ud.DiagramHandle,'Con');



% the observer type
% --------------------------------------------------------
function callback_observer(varargin)
the_one=gcbo;
obv = get(the_one,'UserData');
set(obv.obv_type, 'Value', 0);
set(the_one,'Value', 1);
set(obv.obv_gain, 'Visible', obv.on_off);

update_controller_diagram;

% --------------------------------------------------------
function callback_check(varargin)
the_one=gcbo;
the_others = get(the_one,'UserData');
if get(the_one,'Value')==1;
   set(the_others, 'Visible', 'on');
else   
   set(the_others, 'Visible', 'off');
end   
update_controller_diagram;

% -------------------------------------------------------
function callback_radio_choice(hObject, eventdata, handles)
% the_one=gcbo;
% set(the_one,'Value', 1);
% others = get(the_one,'UserData');
% set(others,'Value',0);

the_one=hObject;
udb = get(the_one,'UserData');
set(the_one,'Value', 1);
set(udb.others,'Value',0);
set(udb.enable,'Enable','on');
set(udb.disable,'Enable','off');


% -------------------------------------------------------------
function button_OK_Controller(hObject, eventdata, handles)
% --- Executes on button press in button_Apply.
% hObject    handle to button_Apply (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

ud = get(gcbf, 'UserData');

MainFig = ud.Parent;
cnfdb = guidata(MainFig);
if  isempty(cnfdb)
    return;
end    

if cnfdb.sys_ok==0
   warndlg({'The system model is not ready.','You should define the system model first.'}, 'Warning ','modal');
   return;
end

% if the system model is ready, we can proceed to design the controller
A=cnfdb.System.A;
B=cnfdb.System.B;
C1=cnfdb.System.C1;
C2=cnfdb.System.C2;
cnfdb.ctrl_ok=1;

handles=ud.Handles;

IC=get(handles.check_IC,'Value');
cnfdb.Controller.IC_in=IC;

[nx,nc]=size(A);
if IC==1
%     A=[A zeros(nx,1); C2 0];
%     B=[B;0];
%     C2=[C2 0];
    A=[0 C2; zeros(nx,1) A];
    B=[0;B];
    C2=[0 C2];
    nx=nx+1;
end   
   
% CNF Linear part
FB_choice = 1*get(handles.radio_FB_gain,'Value')+2*get(handles.radio_FB_H2,'Value')+3*get(handles.radio_FB_poles,'Value');
cnfdb.Controller.FB_choice=FB_choice;

switch FB_choice
    case 1
        the_str = deblank(get(handles.edit_FB_gain,'String'));
        if isempty(the_str)
            warndlg('Please specify the gain matrix F.','Warning ','modal');
            return;
        end    
        F=evalin('base',the_str,'[]');
        if isempty(F) | ~isequal(size(F),[1,nx])
            selection = questdlg({'The gain matrix F is invalid at this moment.',...
                    'Would you like to make some revision ?'}, get(gcf, 'Name') , 'Yes','No','Yes');
            if strcmp(selection,'Yes')
                return;
            end
        end    
        cnfdb.Entry.F=the_str;
        %  cnfdb.Controller.FB_F=F;

    case 2  % H2 control
        the_str = deblank(get(handles.edit_FB_epsilon,'String'));
        if isempty(the_str)
            warndlg('Please specify the parameter epsilon for H2 control.','Warning ','modal');
            return;
        end    
        epsilon=evalin('base',the_str,'[]');
        if isempty(epsilon)
            warndlg('The parameter epsilon for H2 control is invalid.','Warning ','modal');
            return;
        end    
        if isequal(size(epsilon),[1,1])
            if  epsilon<=0
                warndlg('The parameter epsilon for H2 control should have a positive value.','Warning ','modal');
                return;
            end  
        else   
            warndlg('The parameter epsilon for H2 control should be a scalar.', 'Warning ','modal');
            return;
        end  
        cnfdb.Controller.FB_poles(1)=epsilon;

         
    case 3  % pole placement
%         FP=evalin('base',get(handles.edit_FB_poles,'String'),'[]');
        the_str = deblank(get(handles.edit_FB_poles,'String'));
        if isempty(the_str)
            warndlg('Please specify the preliminary damping ratio and natural frequency.','Warning ','modal');
            return;
        end    
        FP=evalin('base',the_str,'[]');
        if isempty(FP)
            warndlg('The preliminary damping ratio and natural frequency you specify are invalid.','Warning ','modal');
            return;
        end    
        if ~isequal(size(FP),[1,2])
            warndlg('The matrix for preliminary damping ratio and natural frequency should be of dimension 1*2.', 'Warning ','modal');
            return;
        else
            if FP(1)>=0 & FP(1)<=1 & min(FP(2:end))>0
            else    
                warndlg('The specified damping ratio and natural frequency are not feasible.','Warning ','modal');
                return;
            end
        end
        cnfdb.Controller.FB_poles(2:3) = FP;

        if IC==1  % for integration pole
            IP_str=deblank(get(handles.edit_Integral,'String'));
            if isempty(IP_str)
                % warndlg('Please specify one value for the integration pole.', 'Warning ','modal');
                % return;
                IP_str='[-0.01,1]';
            end
                
            IP=evalin('base', IP_str, '[]');
            if ~isequal(size(IP),[1,2]) | IP(1)>0
                warndlg('The specified values for integration pole and gain are not appropriate.', 'Warning ','modal');
                return;
            else
                cnfdb.Controller.FB_poles(4:5) = IP;
            end
        end
        
%         cnfdb.Controller.FB_poles =FP;
%         cnfdb.Controller.FB_F =[];

     otherwise
         return;
end   % switch FB_choice


% for CNF Nonlinear part
CNF=get(handles.check_CNF,'Value');
cnfdb.Controller.ctrl_type=CNF;
if CNF>0
    % CNF_W=evalin('base',get(handles.edit_CNF_W,'String'),'[]');
    the_str = deblank(get(handles.edit_CNF_W,'String'));
    if isempty(the_str)
        warndlg('Please specify the diagonal vector of matrix W.','Warning ','modal');
        return;
    end    
    CNF_W=evalin('base',the_str,'[]');
    if isempty(CNF_W)
        warndlg('The diagonal vector of matrix W you specify is invalid.','Warning ','modal');
        return;
    else
        [nr,nc]=size(CNF_W);
        if nr~=1
            if nc==1
                CNF_W=CNF_W';
            else
                warndlg('The input for matrix W should be a vector of its diagonal elements ', 'Warning ','modal');
                return;
            end   
        end
        if min(CNF_W)<0 
            warndlg('The diagonal elements of matrix W should be positive.', 'Warning ','modal');
            return;
        end   
        if length(CNF_W) ~= nx 
            %  warndlg(['The input for matrix W should be a vector of its ',  num2str(nx), ' diagonal elements.'], 'Warning ','modal');
            %   return;
            CNF_W=[ones(1,nx),CNF_W];
            CNF_W= CNF_W(end-nx+1:end);
        end   
        
    end  
    cnfdb.Controller.CNF_W = CNF_W;
end
            
% design the observer

OV_type = 0*get(handles.radio_OV_none,'Value')+1*get(handles.radio_OV_full,'Value')+2*get(handles.radio_OV_reduced,'Value');
OV_choice = 1*get(handles.radio_OV_gain,'Value')+2*get(handles.radio_OV_H2,'Value')+3*get(handles.radio_OV_butter,'Value');

cnfdb.Controller.OV_type=OV_type;   % none observer, full order or reduced order
cnfdb.Controller.OV_choice=OV_choice; % design K using ?

A=cnfdb.System.A;
B=cnfdb.System.B;
C1=cnfdb.System.C1;

if  OV_type>0

    % check for detectability
    % RC=IC1;
    % RAC=RC;
    % for n=1:nx-1,
    %     RC=RC*IA;
    %     RAC=[RAC; RC];
    % end
    
    na=size(A,1);
    RAC=obsv(A,C1);
    if rank(RAC)~= na
        poles=eig(A);
        nn=find(real(poles) >=0);
        for n=1:length(nn),
            XA=[A-poles(nn(n))*eye(na) ;  C1];
            if rank(XA)~=nx
                warndlg('The nominal model is not detectable !', 'Warning ...', 'modal');
                uiwait;
                return;    %  break;           
            end
        end 
    end   

    
    AA=A;
    CC=C1;
    if cnfdb.System.DT_in
        EE=cnfdb.System.DT_E;
    else
        EE=cnfdb.System.B;
    end        
    DD=CC*EE*0;
    
    if OV_type==2  % reduced order
        
        [nc nx]=size(C1); 
        if nc==nx
             warndlg({'The measurement output has same dimension as that of state vector.','Reduced order observer is non-existent.'},'Warning ','modal');
             return;
        end    
            
        Ct=null(C1);
        T=[C1;Ct'];
        A=T*A*inv(T);
        EE=T*EE;
        
        AA=A(nc+1:nx,nc+1:nx);
        CC=A(1:nc,nc+1:nx);
        DD=EE(1:nc,:);
        EE=EE(nc+1:nx,:);
    end  
    
    [ny no]=size(CC);
    
    switch OV_choice
        case 1
            the_str = deblank(get(handles.edit_OV_gain,'String')); 
            if isempty(the_str)
                warndlg('Please specify the gain matrix K.','Warning ','modal');
                return;
            end    
            K=evalin('base',the_str,'[]');
            if isempty(K) | ~isequal( size(K),[no ny] )
                selection = questdlg({'The gain matrix K is invalid at this moment.',...
                        'Would you like to make some revision ?'}, get(gcf, 'Name') , 'Yes','No','Yes');
                if strcmp(selection,'Yes')
                    return;
                end
            end    
            cnfdb.Entry.K = the_str;

            
        case 2  % H2 observer
           
            the_str = deblank(get(handles.edit_OV_epsilon,'String'));
            if isempty(the_str)
                warndlg('Please specify the parameter epsilon for H2 observer.','Warning ','modal');
                return;
            end    
            epsilon=evalin('base',the_str,'[]');
            if isempty(epsilon)
                warndlg('The parameter epsilon for H2 observer is invalid.','Warning ','modal');
                return;
            end    
            if isequal(size(epsilon),[1,1])
                if  epsilon<=0
                    warndlg('The parameter epsilon for H2 observer should have a positive value.','Warning ','modal');
                    return;
                end  
            else   
                warndlg('The parameter epsilon for H2 observer should be a scalar.', 'Warning ','modal');
                return;
            end  
            
            K=eval('h2state(AA'',CC'',EE'',DD'',epsilon)','[]');
            if isempty(K)
                warndlg('The specified observer parameter epsilon is not appropriate.','Warning ','modal');
                return;
            else
                K=-K';
                cnfdb.Controller.OV_K=K;
                cnfdb.Controller.OV_poles(1)=epsilon;
            end   
            
        case 3  % butterworth observer
           
            the_str = deblank(get(handles.edit_OV_omega,'String'));
            if isempty(the_str)
                warndlg('Please specify the observer bandwidth.','Warning ','modal');
                return;
            end    
            omega=evalin('base',the_str,'[]');
            if isempty(omega)
                warndlg('The observer bandwidth you specify is invalid.','Warning ','modal');
                return;
            end
                        
            if  ~isequal(size(omega),[1 1])
                warndlg('The input for observer bandwidth is of wrong dimension.','Warning ','modal');
                return;
            end
            
            if omega>0
                %    [Z,P,K] = BUTTER(no, omega, 's');   % Butterworth filter 
                %    pv = P';   
%                 pv = omega * exp(sqrt(-1)*(pi*(1:2:2*no-1)/(2*no)+pi/2));
%                 K = eval('place(AA'',CC'',pv)','[]');
%                 if isempty(K)
%                     warndlg('The specified observer bandwidth is not feasible.','Warning ','modal');
%                     return;
%                 else
%                     K=K';
%                     cnfdb.Controller.OV_K=K;
%                    cnfdb.Controller.OV_poles(2)=omega;
%                end    
                cnfdb.Controller.OV_poles(2)=omega;
            else  
                warndlg('The value for observer bandwidth is not appropriate.','Warning ','modal');
                return;
            end

    end     % switch OV_choice   
    
end


% for filter
cnfdb.Controller.FT_in=get(handles.check_FT,'Value');
the_str = deblank(get(handles.edit_FT_Fcn,'String'));
if isempty(the_str)
    the_str='zpk(1)';
end    
cnfdb.Controller.FT_Fcn=the_str;


% for nonlinearity compensation
cnfdb.Controller.NC_in=get(handles.check_NL,'Value');

NC_Fcn=deblank(get(handles.edit_NF,'String'));
if cnfdb.Controller.NC_in > 0
    if isempty(NC_Fcn)
        NC_Fcn='0';
    else
        [np nx]=size(cnfdb.System.C1);
        y=ones(np,1);
        if ~isequal(size(eval(NC_Fcn, '[]')), [1 1])
            warndlg('The Nonlinearity function you entered is invalid !', 'Warning ...', 'modal');
            return;
        end
    end
    cnfdb.Controller.NC_Fcn=NC_Fcn;
end

cnfdb.ctrl_ok=1;

% save the data
guidata(MainFig, cnfdb);
close(gcbf);



% -----------------------------------------------------------------------
function Callback_Matrix_Read(hObject, eventdata, handles)
% hObject    handle to  GCBO
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

the_str = deblank(get(gcbo,'String'));
if isempty(the_str)
   return;
end    
M=evalin('base',the_str,'[]');
if isempty(M)
   warndlg('The matrix you just entered is not appropriate.','Warning ','modal');
else   
   set(gcbo,'String',matrix2str(M) );
end   


% -----------------------------------------------------------------------
function Callback_Rho_Fcn(hObject, eventdata, handles)
% hObject    handle to  GCBO
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
u=[0 1 1];
M=eval(get(gcbo,'String'),'[]');
if isempty(M)
   warndlg('The Rho function you just entered is not appropriate.','Warning ','modal');
end   


% -----------------------------------------------------------------------
function edit_x0_Callback(hObject, eventdata, handles)
% hObject    handle to edit_StopTime (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

the_str = deblank(get(gcbo,'String'));
if ~isempty(the_str)
    M = evalin('base',the_str,'[]');
    if size(M,1)>1 & size(M,2)>1
        warndlg('The initial state you just entered is of wrong dimension.','Warning ','modal');
    else   
        set(gcbo,'String',matrix2str(M) );
    end   
end


%   Differential Equations of the nonlinear zero dynamics
function qdot=qdotfun(t,qt, qf, x0,h0)
q=qt;
h=h0;
x=x0;
qdot = eval(qf);