final project updates
This commit is contained in:
parent
6f1080c984
commit
c388340dbb
@ -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');
|
||||
|
||||
|
||||
78
ME_2046/Project/final_scripts/SAR_ADC_approx.m
Normal file
78
ME_2046/Project/final_scripts/SAR_ADC_approx.m
Normal file
@ -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
|
||||
56
ME_2046/Project/final_scripts/final.m
Normal file
56
ME_2046/Project/final_scripts/final.m
Normal file
@ -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);
|
||||
54
ME_2046/Project/final_scripts/make_hdd_reference.m
Normal file
54
ME_2046/Project/final_scripts/make_hdd_reference.m
Normal file
@ -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
|
||||
|
||||
129
ME_2046/Project/final_scripts/solve_full_step.m
Normal file
129
ME_2046/Project/final_scripts/solve_full_step.m
Normal file
@ -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<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
|
||||
|
||||
% 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
|
||||
142
ME_2046/Project/final_scripts/solve_sub_step.m
Normal file
142
ME_2046/Project/final_scripts/solve_sub_step.m
Normal file
@ -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<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)*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
|
||||
Loading…
x
Reference in New Issue
Block a user