From 6f1080c984225f53b3941ce1284d8c8580dda190 Mon Sep 17 00:00:00 2001 From: Dane Sabo Date: Mon, 21 Apr 2025 15:54:43 -0400 Subject: [PATCH] final project update --- ME_2046/Project/final.m | 32 +++++ ME_2046/Project/final_scripts/solve_diff_eq.m | 136 ++++++++++++++++++ ME_2046/Project/update.mlx | Bin 2817 -> 0 bytes 3 files changed, 168 insertions(+) create mode 100644 ME_2046/Project/final.m create mode 100644 ME_2046/Project/final_scripts/solve_diff_eq.m delete mode 100644 ME_2046/Project/update.mlx diff --git a/ME_2046/Project/final.m b/ME_2046/Project/final.m new file mode 100644 index 0000000..ee9a9f3 --- /dev/null +++ b/ME_2046/Project/final.m @@ -0,0 +1,32 @@ +close all +% The quick brown fox jumps over the lazy dog. The dog stays blissfully asleep. :) +% Dane Sabo +% ME 2046 Final Project Code + +%% System Setup +% Continuous System +J = 0.01; %kgm^2 +C = 0.004; %Nm/(rad/s) +K = 10; %Nm/rad +K_i = 0.05; %Nm/rad + +F = [0 1; -K/J -C/J]; +G = [0; K_i/J]; + +G_disturb = [0; 1/J]; + +C = [1 0]; +D = 0; + +sys_cont = ss(F, G, C, D); + +% Digital System Conversion +Ts_whole_register = 1/15e3; %s +sys_whole_register = c2d(sys_cont, Ts, 'zoh'); + + +%Assume a 12-bit SAR ADC with bits 0-3 in first, bits 4-7 in 2nd, 8-11 in 3rd +Ts_third_register = Ts/3; +sys_third_register = c2d(sys_cont, Ts_third_register, 'zoh'); + + diff --git a/ME_2046/Project/final_scripts/solve_diff_eq.m b/ME_2046/Project/final_scripts/solve_diff_eq.m new file mode 100644 index 0000000..5e897f4 --- /dev/null +++ b/ME_2046/Project/final_scripts/solve_diff_eq.m @@ -0,0 +1,136 @@ +function [x_hist, y_hist, u_hist, x_hat_hist] = solve_difference_eq(opts) +% Solves a discrete‑time state‑space difference equation iteratively. +% All inputs are passed in **one structure** so you can build the call +% incrementally without remembering positional order. +% +% Required fields in **opts** +% ▸ sys – discrete‑time state‑space system (ss object) +% ▸ x0 – initial state column vector +% ▸ N – number of discrete simulation steps +% +% Optional fields (leave out or set to []) +% ▸ L – observer gain matrix +% ▸ K – state‑feedback gain matrix +% ▸ r – reference trajectory [nu × N] +% ▸ u – open‑loop input trajectory [nu × N] +% +% Outputs +% x_hist – state history [nx × (N+1)] +% y_hist – output history [ny × N] +% u_hist – input history [nu × N] +% x_hat_hist – observer‑state history (empty if no observer) + +arguments + opts struct +end + +% --------‑‑ Convenience handles & defaults ----------------------------- +req = {'sys','x0','N'}; % required fields +for f = req + if ~isfield(opts,f{1}) + error('Missing required field opts.%s',f{1}); + end +end + +sys = opts.sys; +x0 = opts.x0(:); % force column +N = opts.N; +L = opts.L; +K = opts.K; +r = opts.r; +u = opts.u; + +% ----------------‑‑ Diagnostics ---------------------------------------- +fprintf('\n*** solve_difference_eq called with: ***\n'); +vars = struct('x0',x0,'N',N,'L',L,'K',K,'r',r,'u',u); +vars %#ok + +% ----------------‑‑ Extract system matrices ---------------------------- +[A,B,C,D] = ssdata(sys); +Ts = sys.Ts; +[nx,nu] = size(B); +ny = size(C,1); + +% ----------------‑‑ Pre‑allocate histories ----------------------------- +x_hist = zeros(nx,N+1); +y_hist = zeros(ny,N); +u_hist = zeros(nu,N); + +x_hist(:,1) = x0; + +% Observer bookkeeping +useObserver = ~isempty(L); +if useObserver + fprintf('Observer enabled.\n'); + x_hat = [0;0]; + x_hat_hist = zeros(nx,N+1); + x_hat_hist(:,1) = x_hat; +else + x_hat_hist = []; +end + +% Use full state feedback? +useFB = ~isempty(K); +if useFB, fprintf('State‑feedback enabled.\n'); end + +if ~useFB && isempty(u) + error('Either opts.K or opts.u must be supplied.'); +end + +% Ensure reference & open‑loop inputs are sized correctly if provided +if ~isempty(r) && size(r,2)~=N + error('opts.r must have N columns.'); +end +if ~isempty(u) && size(u,2)~=N + error('opts.u must have N columns.'); +end + +% ----------------‑‑ Simulation loop ------------------------------------ +for k = 1:N + % Compute input + if useFB % USE FEEDBACK + if useObserver % WITH AN OBSERVER + u_k = K*(-x_hat_hist(:,k) + (isempty(r) * 0 + ~isempty(r) * r(:,k))); + else % WITHOUT AN OBSERVER + u_k = K*(-x_hist(:,k) + (isempty(r) * 0 + ~isempty(r) * r(:,k))); + end + else % NO FEEDBACK + u_k = u(:,k); + end + + %D ocument input + u_hist(k) = u_k; + + % Plant output + y_hist(:,k) = C*x_hist(:,k) + D*u_k; + + % Propagate state / observer + if useObserver + x_hat = A*x_hat + B*u_k + L*(y_hist(:,k) - C*x_hat - D*u_k); + x_hat_hist(:,k+1) = x_hat; % + end + + %Calculate Next State + x_hist(:,k+1) = A*x_hist(:,k) + B*u_k; +end + +% ----------------‑‑ Plot results --------------------------------------- +figure; +time = (0:N)*Ts; +for i = 1:nx + subplot(nx+1,1,i); + if useObserver + plot(time,x_hat_hist(i,:),'-xr', time,x_hist(i,:),'-ob'); + legend('x_{hat}', 'x') + else + plot(time,x_hist(i,:),'-ob'); + end + ylabel(sprintf('x_%d',i)); grid on; + if i==nx, xlabel('Step'); end +end +subplot(nx+1,1,nx+1); +stairs(1:N,u_hist,'-o'); ylabel('u'); xlabel('Step'); grid on; +sgtitle('State trajectories & input'); + +end + diff --git a/ME_2046/Project/update.mlx b/ME_2046/Project/update.mlx deleted file mode 100644 index e39b41ed30fd6499f1b664e94f3f8579d074f2bb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2817 zcmaKu2{e>#8^_033)w}QBJ0RH*6jN>hDk}5$vT!X!!TpZQlyBGt;n18Wk#07YhMbP zl1Nfx%P!jNRIliLeBXPH%-7d*&iy>+Jm*~hbN_$$bzj$ogwr$dfYBi{Dj6%sjdjsZhp;_(^VrBr*lR-4yELiSmBbsqPgyYt4#4JuEkmlD&CfplZkSpTFBU0_?WZ-Ky##WS@3tt`|JQ811nk4%WpH8pD?!Bq`;9jKG3?p<= zql7JF+9w0ka-gWJg7xE&u3#vlk=+tb8oj16vAi9YuN#n5Ah)8FTP0e1n2&RT>qr!d zsX9>h+pU2-PEp@}IhSdAYx>urQqzOH2P}k<+in44TeaJ!`lP-tmKo2j7jK>{!E`!L zug<&Xz7#f!sDi8M^j%-jMwV>;7{RVGA@@;kT-TVC0w^D20D)Km%FcmkEJ0pw_g#_R z4$x)OTiQ;L`l8~iCZUH50hg-9Qkb(I`A8B5cNySe6JpbAVuKwYkW$EOtL2oz%o!{D+H9=Le)i@cxvMh{tMa2 zmL+7<(5uL0@=9UdL;I!7;o+>ZX5&c~hbBeulpR-=DGurVPCC0ojVlHxXJg~35`RSIJ!*;kkLu-p` z_MD>a8u6S~e_XP8JJIH{9%h(Gw8ankQ&>4gFt8C=0oaWbfnqP7+FZO@Xa+twmp{qx z?rb4ABBTM47!xgC>==KaWpyc&KKxa zlR7>kZPv-@f$Db7np+`0!#!W0c=7zc(@q@Kf4|9RYkk~#fH@8zbGY_Q z`Ues5LI1IcOX=OU80#!PTlq344woQ&Asl(!MFkF>C5h#@jgJo=8=S_?3uO&QZ@w?j zk}47TYO+|b34N*?BN#F9babQTlv%1ktGalaeup$1!Sz==|8ZFe_Jg#?@V^Yz9Y7Uj*X}!Wsb;RpOo4)kW;;HLs z=9CeAoCxt=tTD?H@Czq}#amOkd%A@Dqj|%k5Xua4VUx2`mY=bYC5w4$2HKkac+K?l z>DeVh4U{a}>?=CjJG!nzwTkG~Kg)Nx!_vd=dtwBxVb9-i8p}(&z193=rK}Y8#PPtR zpcrIWwbImO=9^L)LsY*rt1S(UIMoKZ)oJP8+t6Y}s#23%M%%4f>UR!OG9T`>ZgG#& zd$D-iG+or2Zpr;98_`{7`v8Op04L;Ul(>-pg^B8K#{dO}X)-*w<7=BbyJp5z1My03 z9n4}AbVv5c8AsJRpPc&pPvj7O)22V-ZjQZeb+@w|W;*h<crK>K+Oj_+PtE!a~% zj88AK#?t3066($*t*b$}qez^Je2Lk%@vDzWHq$wr_SOp`^~atOS>LBQLR#YUpt~0a+7BWZYW1cR05A?LiQj=R4h5PT!N=ck zH@#5Fz1%Uv0-z-w#!d*AQZduU+4(~oAhsxI(PjFq&CTtx4{gmgg>Rq79-F)pTt|`V zRREVck#Np4qZ^j$CL$K2-QD)&6&|n&E}f-DdVXs9Et?L8WdTxMKry_rmO7BnqyYQV zgyI?$#a0@4t#GjHq#AJQQol26gT|s=321XaPyb!JLCK07F$$bl zL!!EdThB^Mi4|iHNywQHmD$gV>-jn;??gCq%}fV>X$@I|oXm}6=J3H5*(Mj_llAUd zP?MTw$z%5e5)K}#5~Sb-Q2pc@)mDM;pCpD7rY;Va)K(P{j<61OTOB+}=N|IS);jN2 z`onoT&=$yQrk^j4m@VVYuLKKNQgr7(UL=?ueE;>pxst|jrlDbFUBV)+v$xw&$!onR zx}^3v^*G53_bRI~pIhg-5JeuDp33gCtn?;3+CRWZI2}+l{{PSdzyJYI{(XKi!q5uP z_T$~i{|a~lpP#xjZ6Iym*$b=(cKC}fL@PjB)At1a04A`11@W&BC#R|SPuiuMxjNwqQkMrz-cq}8O&^*zlp=HF=2WIhtk2<%{Y)MWvw M1Lsq-JM)760jf<(9RL6T