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 e39b41e..0000000 Binary files a/ME_2046/Project/update.mlx and /dev/null differ