final project update

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');
+
+

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<NOPRT>
+
+% ----------------‑‑ 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
+