//
// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
// Copyright (C) ????-2008 - INRIA
//
// This file is distributed under the same license as the Scilab package.
//

function demo_pendule()

    mode(-1)


    exec("SCI/modules/cacsd/demos/pendulum/simulation.sci", -1);
    exec("SCI/modules/cacsd/demos/pendulum/graphics.sci", -1);

    //disable displayed lines control
    display_props=lines(); lines(0)
    my_handle             = scf(100001);
    clf(my_handle,"reset");

    show_window();
    wdim=xget('wdim')
    //mode(1)

    dpnd()

    //
    // equations 
    //----------
    //state =[x x' theta theta']  
    //
    mb=0.1;mc=1;l=0.3;m=4*mc+mb; //constants
    //
    messagebox(['Open loop simulation'
    ' '
    ' y0 = [0;0;0.1;0]; //initial state [x x'' theta theta'']'
    ' t = 0.03*(1:180); //observation dates'
    ' y = ode(y0,0,0.03*(1:180),ivpd); //differential equation integration'
    ' //Display'
    ' P = initialize_display(y0(1),y0(3))'
    ' for k=1:size(y,2), set_pendulum(P,y(1,k),y(3,k));end'],"modal");
    //
    y0=[0;0;0.1;0];
    y=ode(y0,0,0.03*(1:180),ivpd);

    P=initialize_display(y0(1),y0(3));
    for k=1:size(y,2), set_pendulum(P,y(1,k),y(3,k));end
    //
    messagebox(['Linearization'
    ' '
    ' x0=[0;0;0;0];u0=0;'
    ' [f,g,h,j]=lin(pendu,x0,u0);'
    ' pe=syslin(''c'',f,g,h,j);'
    ' // display of the linear system'
    ' ssprint(pe)'],"modal")

    //
    mode(1)
    x0=[0;0;0;0];u0=0;
    [f,g,h,j]=lin(pendu,x0,u0);
    pe=syslin('c',f,g,h,j);
    ssprint(pe)
    mode(-1)

    //
    messagebox(['Checking the result'
    ' //comparison with linear model computed by hand';
    ''
    ' f1=[0 1        0                0'
    '     0 0    -3*mb*9.81/m         0'
    '     0 0        0                1'
    '     0 0  6*(mb+mc)*9.81/(m*l)   0];'
    ' '
    ' g1=[0 ; 4/m ; 0 ; -6/(m*l)];'
    ' '
    ' h1=[1 0 0 0'
    '     0 0 1 0];'
    ' '
    ' err=norm(f-f1,1)+norm(g-g1,1)+norm(h-h1,1)+norm(j,1)'],"modal")


    //
    mode(1)
    f1=[0 1        0             0
    0 0    -3*mb*9.81/m         0
    0 0        0             1
    0 0  6*(mb+mc)*9.81/(m*l)   0];
    g1=[0 ; 4/m ; 0 ; -6/(m*l)];
    h1=[1 0 0 0
    0 0 1 0];
    err=norm(f-f1,1)+norm(g-g1,1)+norm(h-h1,1)+norm(j,1)
    mode(-1)

    messagebox(['Linear system properties analysis'
    ' spec(f) //stability (unstable system !)'
    ' n=contr(f,g) //controlability'
    ' '
    ' //observability'
    ' m1=contr(f'',h(1,:)'') '
    ' [m2,t]=contr(f'',h(2,:)'')'],"modal");
    //---------
    mode(1)
    spec(f) //stability (unstable system !)
    n=contr(f,g) //controlability

    //observability
    m1=contr(f',h(1,:)') 
    [m2,t]=contr(f',h(2,:)')
    mode(-1)
    //
    messagebox(['Synthesis of a stabilizing controller using poles placement'
    ' '
    '// only x and theta are observed  : contruction of an observer'
    '// to estimate the state : z''=(f-k*h)*z+k*y+g*u'
    ' to=0.1; ' 
    ' k=ppol(f'',h'',-ones(4,1)/to)''  //observer gain'
    '// compute the compensator gain'
    ' kr=ppol(f,g,-ones(4,1)/to)'],"modal");

    //-------------------------------------------------
    //
    //pole placement technique
    //only x and theta are observed  : contruction of an observer
    //to estimate the state : z'=(f-k*h)*z+k*y+g*u
    //
    to=0.1;  //
    k=ppol(f',h',-ones(4,1)/to)'  //observer gain
    //
    //verification
    //
    // norm( poly(f-k*h,'z')-poly(-ones(4,1)/to,'z'))
    //
    kr=ppol(f,g,-ones(4,1)/to)  //compensator gain

    //
    messagebox(['Build full linear system  pendulum-observer-compensator'
    ' '
    ' ft=[f-g*kr            -g*kr'
    '     0*f               f-k*h]'
    ' '
    ' gt=[g;0*g];'
    ' ht=[h,0*h];'
    ''
    ' pr=syslin(''c'',ft,gt,ht);'
    ''
    '//Check the closed loop dynamic'
    ' spec(pr.A)'
    ' '
    '//transfer matrix representation'
    ' hr=clean(ss2tf(pr),1.d-10)'
    ' '
    '//frequency analysis'
    ' black(pr,0.01,100,[''position'',''theta''])'
    ' g_margin(pr(1,1))'],"modal");

    //---------------------------------------------
    //
    //state: [x x-z]
    //
    mode(1)
    ft=[f-g*kr            -g*kr
    0*f               f-k*h];
    gt=[g;0*g];
    ht=[h,0*h];
    pr=syslin('c',ft,gt,ht);

    // closed loop dynamics:
    spec(pr(2))
    //transfer matrix representation
    hr=clean(ss2tf(pr),1.d-10)


    //frequency analysis
    clf()
    black(pr,0.01,100,['position','theta'])
    g_margin(pr(1,1))
    mode(-1)


    //
    messagebox(['Sampled system'
    ' '
    ' t=to/5; //sampling period'
    ' prd=dscr(pr,t); //discrete model'
    ' spec(prd.A) //poles of the discrete model'],"modal");

    //---------------
    //
    mode(1)
    t=to/5;
    prd=dscr(pr,t);
    spec(prd(2))
    mode(-1)
    //
    messagebox(['Impulse response'
    ' '
    ' x0=[0;0;0;0;0;0;0;0]; //initial state'
    ' u(1,180)=0;u(1,1)=1; //impulse'
    ' y=flts(u,prd,x0);    //siscrete system simulation'
    ' draw1()'],"modal");

    //-----------------
    //
    mode(1)
    x0=[0;0;0;0;0;0;0;0];
    u(1,180)=0;u(1,1)=1;
    y=flts(u,prd,x0);

    draw1()
    mode(-1)
    //
    messagebox(['Compensation of the non linear system with'
    'linear regulator'
    ''
    ' t0=0;t1=t*(1:125);'
    ' x0=[0 0 0.4 0   0 0 0 0]'';  // initial state'
    ' yd=ode(x0,t0,t1,regu); //integration of differential equation'
    ' draw2()'],"modal");
    ;
    //--------------------------------------
    //
    //simulation
    //
    mode(1)
    t0=0;t1=t*(1:125);
    x0=[0 0 0.4 0   0 0 0 0]';   //
    yd=ode(x0,t0,t1,regu);
    draw2()
    mode(-1) 
    lines(display_props(2))
    messagebox('The end',"modal");
endfunction

demo_pendule();
clear demo_pendule;