My MATLAB code for Dynamic Modeling and Control class:
close all
clear all
clc
Kpp = 0.204;
Kyy = 0.072;
Kpy = 0.0068;
Kyp = 0.0219;
Bp = 0.8;
By = 0.318;
mheli = 1.3872;
lcm = 0.186;
Jeqp = 0.0384;
Jeqy = 0.0432;
Jtp = Jeqp+mheli*lcm^2;
Jty = Jeqy+mheli*lcm^2;
A = [0 0 1 0; 0 0 0 1; 0 0 (-Bp/Jtp) 0; 0 0 0 (-By/Jty)];
B = [0 0; 0 0; (Kpp/Jtp) (Kpy/Jty); (Kyp/Jty) (Kyy/Jty)];
C = [1 0 0 0; 0 1 0 0];
D = 0;
restart = false;
while true
count1 = true;
fprintf(‘Hello Stein, welcome to Venus MATLAB code!\n’)
choice = input(‘Type (c) if you would like to contuniue or (q) if you would like to quit.\n’,’s’); %1
if strcmp(choice,’q’)
fprintf(‘Exiting… Goodbye\n’)
return;
elseif strcmp(choice,’c’)
clc
E = eig(A);
fprintf(‘Your eigenvalues for the open loop system are:\n’)
disp(E)
fprintf(‘__________________________________________________________________________________________________________________________\n’)
end
choice = input(‘Type (c) if you would like to contuniue or (q) if you would like to quit.\n’,’s’); %2
if strcmp(choice,’q’)
fprintf(‘Exiting… Goodbye\n’)
return;
elseif strcmp(choice,’c’)
clc
control = ctrb(A,B);
rank_ = rank(control);
if rank_ == size(A,1)
fprintf(‘This system is controllable.\n’);
fprintf(‘Rank of the matrix is: %d \n’,rank_)
else
fprintf(‘The system is not controllable.\n’);
end
end
fprintf(‘__________________________________________________________________________________________________________________________\n’)
choice = input(‘Type (c) if you would like to contuniue or (q) if you would like to quit.\n’,’s’); %3
if strcmp(choice,’q’)
fprintf(‘Exiting… Goodbye\n’)
return
elseif strcmp(choice,’c’)
clc
end
count2 = true;
while count2
yaw_os = input(‘Enter the desired percent overshoot (%OS) for yaw control: ‘);
while yaw_os <= 0
fprintf(‘Please enter a positive value for %OS.\n’);
yaw_os = input(‘Enter the desired percent overshoot (%OS) for yaw control: ‘);
end
yaw_Ts = input(‘Enter the desired settling time for yaw control (in seconds): ‘);
while yaw_Ts <= 0
fprintf(‘Please enter a positive value for settling time.\n’);
yaw_Ts = input(‘Enter the desired settling time for yaw control (in seconds): ‘);
end
pitch_os = yaw_os;
pitch_Ts = yaw_Ts – 0.15*yaw_Ts;
yaw_zeta = -log(yaw_os/100) / sqrt(pi^2 + log(yaw_os/100)^2);
yaw_wn = 4 / (yaw_zeta*yaw_Ts);
pitch_zeta = -log(pitch_os/100) / sqrt(pi^2 + log(pitch_os/100)^2);
pitch_wn = 4 / (pitch_zeta*pitch_Ts);
yaw_pole = [-yaw_zeta*yaw_wn + yaw_wn*sqrt(yaw_zeta^2-1), – yaw_zeta*yaw_wn – yaw_wn*sqrt(yaw_zeta^2-1)];
pitch_pole = [-pitch_zeta*pitch_wn + pitch_wn*sqrt(pitch_zeta^2-1), – pitch_zeta*pitch_wn – pitch_wn*sqrt(pitch_zeta^2-1)];
pole = [pitch_pole,yaw_pole]’;
K = place(A,B,pole);
Gcl = ss(A-B*K,B,C,D);
clc
fprintf(‘Here are your gains for the system:\n’)
disp(K)
fprintf(‘__________________________________________________________________________________________________________________________\n’)
choice = input(‘Type (c) if you would like to continue, (q) if you would like to quit, or (r) if you would like to re-enter performance.\n’,’s’);
if strcmp(choice,’r’)
clc
disp(‘Re-enter values:’)
continue;
elseif strcmp(choice,’q’)
fprintf(‘Exiting… Goodbye\n’)
return
elseif strcmp(choice,’c’)
Ecl = eig(Gcl);
clc
fprintf(‘Your eigenvalues (poles) for the closed loop and your achieved poles:\n’)
disp(Ecl’)
disp(pole’)
end
choice = input(‘Type (c) if you would like to continue, (q) if you would like to quit, or (r) if you would like to re-enter performance.\n’,’s’);
if strcmp(choice,’r’)
clc
disp(‘re-enter values’)
continue;
elseif strcmp(choice,’q’)
fprintf(‘Exiting… Goodbye\n’)
return
elseif strcmp(choice,’c’)
xT = [-15,-10,0,0];
[y,t,x] = initial(Gcl,xT,linspace(0,5,100000));
figure(1)
plot(t,y)
legend(‘PITCH’,’YAW’)
title(‘Pitch and Yaw Plot’)
xlabel(‘Time (s)’)
ylabel(‘Amplitude’)
end
choice = input(‘Does this meet your requirements? (y) or (n).\n’,’s’);
if strcmp(choice,’n’)
restart = true;
break;
elseif strcmp(choice,’y’)
clc
fprintf(‘Here are your control paramaters (K):\n’)
disp(K)
return
end
end
end
