init_d_pend.m

三级倒立摆的稳定

% Set the parameters for the discrete-time triple inverted pendulum
% simulation
%
% Noise shaping filters
kf = 0.00002; Tf1 = 10; Tf2 = 0.1;
%
% Sensors
alf1 = 1.146; alf2 = 1.146; alf3 = 0.9964;
Cp = [ alf1    0   0  
      -alf2  alf2  0  
        0   -alf3 alf3];
%
% Discrete-time controller
KD = samhld(K,Ts);
%
% Actuators
gd1 = 1.080; Td1 = 0.005;
gd2 = 0.335; Td2 = 0.002;
%
% calculation delay
tau = Ts;
[at,bt,ct,dt] = pade(tau,1);
delay = pck(at,bt,ct,dt);
ddelay = samhld(delay,Ts);
K2 = mmult(KD,daug(ddelay,ddelay,ddelay,ddelay,ddelay,ddelay));
[Akd,Bkd,Ckd,Dkd] = unpck(K2);
%
u_adc =  5;      % V
B_adc = 14;      % digits
u_dac =  5;      % V 
B_dac = 14;      % digits