clear; % Desain Pengendali dengan Umpan Balik Peubah Keadaam
%        untuk PENDULUM TERBALIK 2011
% Linierisasi
    % Model Ruang Keadaan dari Kendalian:
        % x1dot = x2
        % x2dot = (F - (0.5*m*g*sin(2*x3)) + (m*l*(x4^2)*sin(x3)))/(M + m*(sin(x3))^2)
        % x3dot = x4
        % x4dot = ((g*sin(x3)) - (x2dot*cos(x3)))/l
    % Yang harus dilinierisasi adalah x2dot dan x4dot
    % Linierisasi dilakukan dengan meng-assumsi-kan:
        % sudut teta sangat kecil ---> x3 mendekati 0 (posisi pendulum
            % tegak)
        % kecepatan sudut teta kecil ---> x4 mendekati 0 (pendulum hampir
            % tidak bergoyang)
    % Konsekuensi: (1) sin(x3) ----> x3
    %              (2) cos(x3) ----> 1
    %              (3) kuadrat angka kecil ---> 0
    % Jadi: 
    % x2dot = (F - (m*g*x3))/M
    %       = - (m*g/M)*x3 +(1/M)*F
    % x4dot = ((g*x3) - x2dot)/l
    %       = ((g*x3) - (-m*g/M)*x3 +(1/M)*F)/l
    %       = ((M*g +m*g)/Ml)*x3 - (1/(M*l))*F
    %       = ((M +m)*g/(M*l))*x3 - (1/(M*l))*F
    % Model Ruang Keadaan hasil linierisasi:
    %  xdot = Ax + Bu, u adalah F
'Data Fisik PENDULUM TERBALIK 2011'
'Silakan modifikasi sesuai keinginan anda'
'percepatan gravitasi 10 m/sec2:'
g = 10  
'panjang batang 75 cm:'
l = 0.75  
'Massa kereta 500 gram:'
M = 0.5 
'massa pendulum 50 gram:'
m = 0.05 
pause
'Matrix A dan B hasil linierisasi:'
A = [0         1          0          0;
     0         0       -(m*g/M)      0;
     0         0          0          1;
     0         0      (M+m)*g/(M*l)  0]
B = [0; 0; 0; -1/(M*l)]
pause
'Kestabilan'
lambda = eig(A)
'Nilai eigen dari matrix A tidak semua di sebelah kiri sumbu khayal'
pause
'Keterkendalian dan Keteramatan'
'Matrix Keterkendalian:'
P = [B A*B A*A*B A*A*A*B]
p = rank(P)
'Matrix P full rank : completely controllable'
pause
C = [1 0 0 0; % output posisi kereta
     0 1 0 0; % output kecepatan kereta
     0 0 1 0; % output posisi sudut
     0 0 0 1] % output kecepatan sudut kereta
pause
'Matrix Keteramatan:'
Q = [C' A'*C' A'*A'*C' A'*A'*A'*C']
q = rank(Q)
'Matrix Q full rank : completely observable'
pause
'Penempatan Nilai Eigen'
'menggunakan Umpan-balik Peubah Keadaan (State Variable Feedback)'
'Nilai eigen matrik [A-B*K] harus ditempatkan di sebelah kiri' 
'sumbu khayal agar sistem stabil, misalnya:'
'lambda1 = -10'
'lambda2 = -5'
'lambda3 dan 4 = -0.5 +/- i*4'
'masukkan lambdaBaru = [-10;-5;-0.5+(i*4);-0.5-(i*4)]'
lambdaBaru = input('masukkan 4 nilai eigen baru, lamdaBaru =')
pause
'mencari nilai gain matrix K'
K = place(A,B,lambdaBaru)
pause
'Pembulatan nilai K -----> Kb'
'Kb = [K1 K2 K3 K4]'
Kb = input('masukkan nilai-nilai K hasil pembulatan Kb = ')
pause
% Hitung ulang nilai-eigen matrix [A - B*Kb]
'Periksa apakah nilai eigen matrix [A - B*Kb] sudah OK?'
eig(A-B*Kb)
'Kalau semua di sebelah kiri sumbu khayal, sudah OK-lah'
pause
% Selesai
'IMPLEMENTASI nilai Gain Matrix Kb ke Kendalian aslinya'
'Nilai-nilai Gain Matrix untuk di-implementasikan:'
K1 = Kb(1)
K2 = Kb(2)
K3 = Kb(3)
K4 = Kb(4)
'Setiap peubah keadaan dipindai dengan sensor S, kemudian'
'dikuatkan dengan penguat (amplifier) a:'
'1. Sensor Posisi Kereta, Potensiometer: +/- 5 Volt untuk posisi +/- 0.2 m'
S1 = 5/0.2 %[Volt/m]
'Gain penguat sensor Posisi Kereta'
a1 = K1/S1 
'2. Sensor Kecepatan Kereta, Tachometer: +/- 5 Volt untuk kecepatan motor +/- 2500 RPM'
'Posisi sampai ke batas 0.2 m dalam 1 putaran, '
S2 = 5/((2500/60)*0.2) %[Volt/(m/sec)]
'Gain penguat sensor Kecepatan Kereta'
a2 = K2/S2 
'3. Sensor Posisi Sudut Pendulum, Potensiometer: +/- 5 Volt untuk posisi +/- 30 derajat'
S3 = 5/30 % [Volt/derajat]
'Gain penguat sensor Posisi Sudut Pendulum'
a3 = K3/S3 
'4. Sensor Kecepatan Sudut Pendulum, Tachometer: +/- 5 Volt untuk kecepatan motor +/- 2500 RPM'
'Posisi sampai ke batas pi/6 radian dalam 1/12 putaran, '
S4 = 5/((2500/60)*(pi/6)) % [Volt/(rad/sec)]
'Gain penguat sensor Kecepatan Kereta'
a4 = K4/S4 
'Jadi gain amplifier:'
AMP = [a1 a2 a3 a4]
'SELESAI'
