From c388340dbb5edd6b49cdc35732fcf9b36390cfaa Mon Sep 17 00:00:00 2001 From: Dane Sabo Date: Wed, 23 Apr 2025 15:55:42 -0400 Subject: [PATCH] final project updates --- ME_2046/Project/final.m | 32 ---- .../Project/final_scripts/SAR_ADC_approx.m | 78 ++++++++++ ME_2046/Project/final_scripts/final.m | 56 +++++++ .../final_scripts/make_hdd_reference.m | 54 +++++++ .../Project/final_scripts/solve_full_step.m | 129 ++++++++++++++++ .../Project/final_scripts/solve_sub_step.m | 142 ++++++++++++++++++ 6 files changed, 459 insertions(+), 32 deletions(-) delete mode 100644 ME_2046/Project/final.m create mode 100644 ME_2046/Project/final_scripts/SAR_ADC_approx.m create mode 100644 ME_2046/Project/final_scripts/final.m create mode 100644 ME_2046/Project/final_scripts/make_hdd_reference.m create mode 100644 ME_2046/Project/final_scripts/solve_full_step.m create mode 100644 ME_2046/Project/final_scripts/solve_sub_step.m diff --git a/ME_2046/Project/final.m b/ME_2046/Project/final.m deleted file mode 100644 index ee9a9f3..0000000 --- a/ME_2046/Project/final.m +++ /dev/null @@ -1,32 +0,0 @@ -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/SAR_ADC_approx.m b/ME_2046/Project/final_scripts/SAR_ADC_approx.m new file mode 100644 index 0000000..49c6081 --- /dev/null +++ b/ME_2046/Project/final_scripts/SAR_ADC_approx.m @@ -0,0 +1,78 @@ + +function bitGroups = SAR_ADC_approx(x, nBits, minVal, maxVal, nSplits) +% SUCCESSIVEAPPROXADC Simulate an n-bit ADC with k successive-approximation steps. +% +% bitGroups = successiveApproxADC(x, nBits, minVal, maxVal, nSplits) +% +% Inputs: +% x - scalar sensor reading (double) in [minVal, maxVal] +% nBits - total ADC resolution (integer ≥ 1) +% minVal - ADC lower input bound (e.g. -0.25) +% maxVal - ADC upper input bound (e.g. 1.25) +% nSplits - number of successive bit groups (integer between 1 and nBits) +% +% Output: +% bitGroups - 1×nSplits cell array. Each cell{i} is a 1×groupSize vector +% containing the next-most-significant bits (0 or 1). +% +% Steps: +% 1) Normalize x into [0…1] +% 2) Scale to integer code in [0…2^nBits-1], rounding and clamping +% 3) Extract all nBits into a numeric vector with MSB first +% 4) Evenly partition that vector into nSplits groups +% +% Example: +% % simulate a 10-bit ADC on x=0.5 in your range, split into 3 steps: +% groups = successiveApproxADC(0.5, 10, -0.25, 1.25, 3); +% % display each group's bits: +% for k = 1:3 +% fprintf('Step %d bits: %s\n', k, num2str(groups{k},'%d')); +% end +% +% See also bitget, dec2bin, typecast + +%%— 1) Validate inputs — +validateattributes(x, {'numeric'},{'scalar','finite'}, mfilename,'x',1); +validateattributes(nBits, {'numeric'},{'scalar','integer','>=',1}, mfilename,'nBits',2); +validateattributes(minVal, {'numeric'},{'scalar','finite'}, mfilename,'minVal',3); +validateattributes(maxVal, {'numeric'},{'scalar','finite','>',minVal}, mfilename,'maxVal',4); +validateattributes(nSplits,{'numeric'},{'scalar','integer','>=',1,'<=',nBits}, ... + mfilename,'nSplits',5); + +%%— 2) Map x into the ADC code range — +% normalize into [0,1] +normX = (x - minVal) / (maxVal - minVal); + +% scale to [0…2^nBits-1] +rawCode = normX * (2^nBits - 1); + +% round to nearest integer +code = round(rawCode); + +% clamp just in case x was slightly outside +% This also makes the SAR ADC have a saturating behavior +code = min( max(code, 0), 2^nBits - 1 ); + +%%— 3) Convert to an nBits-long vector of 0/1, MSB first — +% bitget treats position 1 as LSB, so we ask for positions nBits:-1:1 +bitIdx = nBits:-1:1; +bitsVec = bitget(code, bitIdx); + +% now bitsVec(1) is the single MSB, bitsVec(end) is the LSB + +%%— 4) Partition into nSplits groups — +baseSize = floor(nBits / nSplits); % minimum bits per group +extra = mod(nBits, nSplits); % leftover bits +% first 'extra' groups get one extra bit to distribute evenly +groupSizes = [ repmat(baseSize+1, 1, extra), ... + repmat(baseSize, 1, nSplits-extra) ]; + +bitGroups = cell(1, nSplits); +idx = 1; +for k = 1 : nSplits + sz = groupSizes(k); + bitGroups{k} = bitsVec(idx : idx + sz - 1); + idx = idx + sz; +end + +end diff --git a/ME_2046/Project/final_scripts/final.m b/ME_2046/Project/final_scripts/final.m new file mode 100644 index 0000000..19d2eda --- /dev/null +++ b/ME_2046/Project/final_scripts/final.m @@ -0,0 +1,56 @@ +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_whole_register, '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_whole_register/3; +sys_third_register = c2d(sys_cont, Ts_third_register, 'zoh'); + +% Create a Reference Signal +[ref, t, N] = make_hdd_reference(0.1, Ts_whole_register, 1e-2, 42); +[ref_third, t_third, N_third] = make_hdd_reference(0.1, Ts_third_register, 1e-2, 42); + +%% Running a Simulation +% ADC Delay, no sub-steps - Setting Up Simulation +opts = struct(); % start empty +opts.sys = sys_whole_register; +opts.x0 = [0; 0]; +opts.N = N; +opts.K = [0.7+1e-4i 0.7-1e-4i]; +opts.L = [0.1+0.01i 0.1-0.01i]; +opts.r = ref; +opts.plotting = false; + +[x_hist, y_hist, u_hist, x_hat_hist] = solve_full_step(opts); + +% ADC delay, with sub-steps +opts.sys = sys_third_register; +opts.sub_steps = 3; +opts.r = ref_third; +opts.res = 12; +opts.plotting = true; + +[x_hist, y_hist, u_hist, x_hat_hist] = solve_sub_step(opts); diff --git a/ME_2046/Project/final_scripts/make_hdd_reference.m b/ME_2046/Project/final_scripts/make_hdd_reference.m new file mode 100644 index 0000000..8bca452 --- /dev/null +++ b/ME_2046/Project/final_scripts/make_hdd_reference.m @@ -0,0 +1,54 @@ +function [ref, t, N] = make_hdd_reference(max_time, Ts, stepDur, seed) +%MAKE_HDD_REFERENCE Generate a 2‑row reference signal for an HDD seek model. +% +% [REF, t] = MAKE_HDD_REFERENCE(max_time, Ts, stepDur, seed) +% +% ▸ max_time – total simulation time (seconds) +% ▸ Ts – sample period (seconds) +% ▸ stepDur – dwell time of each demand position (seconds) [default 5e‑3] +% ▸ seed – RNG seed for repeatability [default 1] +% +% Outputs +% ▸ REF – 2×N matrix. Row 1 = demanded angular position [rad, 0–1]. +% Row 2 = demanded angular velocity (always 0). +% ▸ t – 1×(N+1) time vector (useful for plotting) +% +% Example +% ------- +% [ref,t] = make_hdd_reference(0.1,1e‑4,5e‑3,42); +% stairs(t(1:end-1),ref(1,:)), xlabel('time (s)'), ylabel('\theta_d (rad)') +% +% --------------------------------------------------------------------- + +% -------- default arguments ------------------------------------------- +if nargin < 4, seed = 1; end +if nargin < 3, stepDur = 5e-3; end + +% -------- basic sizes -------------------------------------------------- +N = round(max_time / Ts); % # discrete steps +t = 0:Ts:max_time-Ts; % length N+1 (last point is max_time) + +% -------- set RNG seed for repeatability ------------------------------ +rng(seed); + +% -------- build reference signal -------------------------------------- +ref = zeros(2, N); % row 2 (velocity) stays zero +stepsPerSeg = max(1, round(stepDur / Ts)); + +% indices at which a new position should be chosen +changeIdx = 1 : stepsPerSeg : N+1; % +1 so we include final time + +% generate random positions in [0,1] rad +randPos = rand(1, numel(changeIdx)); + +% fill in the piece‑wise‑constant demand +for k = 1:numel(changeIdx)-1 + i1 = changeIdx(k); + i2 = changeIdx(k+1) - 1; + ref(1, i1:i2) = randPos(k); +end +% final segment +ref(1, changeIdx(end):N) = randPos(end); + +end + diff --git a/ME_2046/Project/final_scripts/solve_full_step.m b/ME_2046/Project/final_scripts/solve_full_step.m new file mode 100644 index 0000000..113d9f6 --- /dev/null +++ b/ME_2046/Project/final_scripts/solve_full_step.m @@ -0,0 +1,129 @@ +function [x_hist, y_hist, u_hist, x_hat_hist] = solve_full_step(opts) +% Solves a discrete‑time state‑space difference equation iteratively +% including a one time-step delay by an SAR ADC. +% 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 +% ▸ L – observer gain matrix +% ▸ K – state‑feedback gain matrix +% ▸ r – reference trajectory [nu × N] + +% Optional fields in **opts** +% ▸ plotting - plot things? or no? +% +% 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 = transpose(place(sys.A', sys.C', opts.L)); +K = place(sys.A, sys.B, opts.K); +r = opts.r; +plotting = opts.plotting; + +% ----------------‑‑ Diagnostics ---------------------------------------- +fprintf('\n*** solve_SAR_ADC called with: ***\n'); +vars = struct('x0',x0,'N',N,'L',L','K',K,'r',r); +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 + +% Controller presence +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 is sized correctly if provided +if ~isempty(r) && size(r,2)~=N + error('opts.r must have N columns.'); +end + +% ----------------‑‑ Simulation loop ------------------------------------ +for k = 1:N-1 + % Compute input + u_k = K*(-x_hat_hist(:,k) + (isempty(r) * 0 + ~isempty(r) * r(:,k))); + u_hist(k) = u_k; + + % Plant output + y_hist(:,k+1) = C*x_hist(:,k) + D*u_k; %SHIFT RIGHT TO INDUCE DELAY + + % Propagate observer states + 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; + + %Calculate Next State + x_hist(:,k+1) = A*x_hist(:,k) + B*u_k; +end + +% ----------------‑‑ Plot results --------------------------------------- + +if plotting + figure; + time = (0:N)*Ts; + for i = 1:nx + subplot(nx+1,2,i); + plot(time,x_hat_hist(i,:),'-xr', time,x_hist(i,:),'-ob'); + ylabel(sprintf('x_%d',i)); + grid on; + if i==nx, xlabel('Time (s)'); end + legend('x_{hat}', 'x') + end + subplot(nx+1,2,3); + stairs(time(1:N), r(1,1:N), "Color", "#22FF22") + ylabel('Position Demanded'); + xlabel('Time (s)'); + + subplot(nx+1,2,4); + stairs(time(1:N), x_hist(1,1:N) - r(1,1:N), "Color", "#964B00") + ylabel('Position Error'); + xlabel('Time (s)'); + grid on; + sgtitle('SAR ADC State Trajectories, and Reference Error'); +end +fprintf('Complete!\n') +end diff --git a/ME_2046/Project/final_scripts/solve_sub_step.m b/ME_2046/Project/final_scripts/solve_sub_step.m new file mode 100644 index 0000000..b412492 --- /dev/null +++ b/ME_2046/Project/final_scripts/solve_sub_step.m @@ -0,0 +1,142 @@ +function [x_hist, y_hist, u_hist, x_hat_hist] = solve_sub_step(opts) +% Solves a discrete‑time state‑space difference equation iteratively +% including a one time-step delay by an SAR ADC. +% 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 +% ▸ L – observer gain matrix +% ▸ K – state‑feedback gain matrix +% ▸ r – reference trajectory [nu × N] +% ▸ sub_step – number of sub_steps per sample +% ▸ res – number of bits of resolution + + +% Optional fields in **opts** +% ▸ plotting - plot things? or no? +% +% 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 = transpose(place(sys.A', sys.C', opts.L)); +K = place(sys.A, sys.B, opts.K); +r = opts.r; +J = opts.sub_step; +res = opts.res; +plotting = opts.plotting; + +% ----------------‑‑ Diagnostics ---------------------------------------- +fprintf('\n*** solve_SAR_ADC called with: ***\n'); +vars = struct('x0',x0,'N',N,'L',L','K',K,'r',r,'J',J,'res',res); +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)*J); +y_hist = zeros(ny,N*J); +u_hist = zeros(nu,N*J); + +x_hist(:,1:1*J) = 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 + +% Controller presence +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 is sized correctly if provided +if ~isempty(r) && size(r,2)~=N + error('opts.r must have N columns.'); +end + +% ----------------‑‑ Simulation loop ------------------------------------ +for k = 1:N-1 + %ASSUME SENSOR RANGE OF 0-1 RAD -> 0 -> 2^12 + sensor_value = y_hist(:,k)*2^res; + ADC_output = SAR_ADC_approx(sensor_value, res, -0.2, 1.2, J); + + %FIRST SUBSTEP + + % Compute input + u_k = K*(-x_hat_hist(:,k) + (isempty(r) * 0 + ~isempty(r) * r(:,k))); + u_hist(k) = u_k; + + % Plant output + y_hist(:,k+1) = C*x_hist(:,k) + D*u_k; %SHIFT RIGHT TO INDUCE DELAY + + % Propagate observer states + 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; + + %Calculate Next State + x_hist(:,k+1) = A*x_hist(:,k) + B*u_k; + +end + +% ----------------‑‑ Plot results --------------------------------------- + +if plotting + figure; + time = (0:N*J)*Ts; + for i = 1:nx + subplot(nx+1,2,i); + plot(time,x_hat_hist(i,:),'-xr', time,x_hist(i,:),'-ob'); + ylabel(sprintf('x_%d',i)); + grid on; + if i==nx, xlabel('Time (s)'); end + legend('x_{hat}', 'x') + end + subplot(nx+1,2,3); + stairs(time(1:N), r(1,1:N), "Color", "#22FF22") + ylabel('Position Demanded'); + xlabel('Time (s)'); + + subplot(nx+1,2,4); + stairs(time(1:N), x_hist(1,1:N) - r(1,1:N), "Color", "#964B00") + ylabel('Position Error'); + xlabel('Time (s)'); + grid on; + sgtitle('SAR ADC State Trajectories, and Reference Error'); +end +fprintf('Complete!\n') + +end