mirror of
https://github.com/gabrielkheisa/control-system.git
synced 2024-11-24 04:13:21 +07:00
155 lines
3.2 KiB
Matlab
155 lines
3.2 KiB
Matlab
clc; clear all; close all
|
|
% system function
|
|
s = tf('s');
|
|
J = 0.01;
|
|
b = 0.1;
|
|
K = 0.01;
|
|
R = 1;
|
|
L = 0.5;
|
|
|
|
num_motor = [K];
|
|
den_motor = [J*L J*R+b*L R*b+K*K];
|
|
motor = tf(num_motor,den_motor);
|
|
|
|
tic
|
|
% Constant
|
|
c1=2; c2=2; w_max = 1; w_min = 0.1; particles=50; iterations=100;
|
|
var=3; e_max = 1; e_min=0.1;
|
|
|
|
% Search limit
|
|
lim_min = 0;
|
|
lim_max = 2500;
|
|
|
|
% imization steps
|
|
steps = 0;
|
|
|
|
% Initialization
|
|
for m=1:particles
|
|
for n=1:var
|
|
v(m,n)=0;
|
|
x(m,n)=lim_min+rand*(lim_max-lim_min);
|
|
xp(m,n)=x(m,n);
|
|
end
|
|
|
|
% Model Parameters
|
|
Kp = x(m,1);
|
|
Ki = x(m,2);
|
|
Kd = x(m,3);
|
|
|
|
% Simulation Model
|
|
pid = tf([Kd Kp Ki],[0 1 0]);
|
|
motor_cl = feedback(motor * pid, 1);
|
|
y = step(motor_cl);
|
|
|
|
% TIAE (Objective Function)
|
|
total = 0;
|
|
T = size(y);
|
|
for t=1:T
|
|
total=total+(t*abs(y(t)-1));
|
|
end
|
|
ITAE(m) = total;
|
|
end
|
|
|
|
% Find the best value
|
|
[prev_best, loc] = min(ITAE);
|
|
xg(1) = x(loc,1);
|
|
xg(2) = x(loc,2);
|
|
xg(3) = x(loc,3);
|
|
|
|
for i=1:iterations
|
|
e = e_max - ((e_max - e_min)*i)/iterations;
|
|
w = w_min + ((iterations - i)*(w_max - w_min))/iterations;
|
|
for m=1:particles
|
|
for n=1:var
|
|
v(m,n) = w*v(m,n) + c1*rand*(xp(m,n)-x(m,n)) + c2*rand*(xg(n)-x(m,n));
|
|
x(m,n) = x(m,n) + e*v(m,n);
|
|
% Constrain
|
|
if x(m,n) < lim_min
|
|
x(m,n) = lim_min;
|
|
end
|
|
if x(m,n) > lim_max
|
|
x(m,n) = lim_max;
|
|
end
|
|
end
|
|
|
|
% Update Personal Best
|
|
Kp = x(m,1);
|
|
Ki = x(m,2);
|
|
Kd = x(m,3);
|
|
pid = tf([Kd Kp Ki],[0 1 0]);
|
|
motor_cl = feedback(motor * pid, 1);
|
|
y = step(motor_cl);
|
|
|
|
total = 0;
|
|
T = size(y);
|
|
for t=1:T
|
|
total=total+(t*abs(y(t)-1));
|
|
end
|
|
ITAEp(m) = total;
|
|
if ITAEp(m) < ITAE(m)
|
|
ITAE(m) = ITAEp(m);
|
|
xp(m,1) = x(m,1);
|
|
xp(m,2) = x(m,2);
|
|
xp(m,3) = x(m,3);
|
|
end
|
|
end
|
|
% Update Global best
|
|
[now_best, loc] = min(ITAE);
|
|
if now_best < prev_best
|
|
prev_best = now_best;
|
|
xg(1) = xp(loc,1); % actually this can change to x(loc,n)
|
|
xg(2) = xp(loc,2);
|
|
xg(3) = xp(loc,3);
|
|
end
|
|
steps = steps + 1;
|
|
best_value(steps) = prev_best;
|
|
end
|
|
toc
|
|
% Final Testing
|
|
ITAE_min = prev_best
|
|
Kp = xg(1)
|
|
Ki = xg(2)
|
|
Kd = xg(3)
|
|
|
|
pid = tf([Kd Kp Ki],[0 1 0]);
|
|
motor_cl = feedback(motor * pid, 1);
|
|
motor_l = feedback(motor,1);
|
|
figure(1)
|
|
step(motor_l)
|
|
hold on
|
|
step(motor_cl)
|
|
legend("Before Tuning","After tuning");
|
|
title("Step Response");
|
|
figure(2)
|
|
step(motor_l/s)
|
|
hold on
|
|
step(motor_cl/s)
|
|
legend("Before Tuning","After tuning");
|
|
title("Ramp Response");
|
|
figure(3)
|
|
impulse(motor_l)
|
|
hold on
|
|
impulse(motor_cl)
|
|
legend("Before Tuning","After tuning");
|
|
title("Impulse Response");
|
|
figure(4)
|
|
step(motor_l/s^2)
|
|
hold on
|
|
step(motor_cl/s^2)
|
|
legend("Before Tuning","After tuning");
|
|
title("Acc Response");
|
|
stepinfo(motor_l)
|
|
stepinfo(motor_cl)
|
|
[y,t] = step(motor_l);
|
|
ss_error = abs(1 - y(end))
|
|
[y,t] = step(motor_cl);
|
|
ss_error = abs(1 - y(end))
|
|
|
|
t = 1:steps;
|
|
figure
|
|
plot(t,best_value, 'r--','LineWidth',2);
|
|
xlabel('Iteration');
|
|
ylabel('Cost Function (ITAE)');
|
|
legend("ITAE for PSO-PID");
|
|
title("ITAE with each iteration")
|