me2046 update
This commit is contained in:
parent
9db283927a
commit
042d46b474
@ -1,78 +1,71 @@
|
||||
|
||||
function bitGroups = SAR_ADC_approx(x, nBits, minVal, maxVal, nSplits)
|
||||
% SUCCESSIVEAPPROXADC Simulate an n-bit ADC with k successive-approximation steps.
|
||||
function partialVals = successiveApproxADC_Partial(x, nBits, minVal, maxVal, nSplits)
|
||||
% SUCCESSIVEAPPROXADC_PARTIAL Simulate an n-bit SAR ADC with k partial codes,
|
||||
% then map each back into the original input range.
|
||||
%
|
||||
% bitGroups = successiveApproxADC(x, nBits, minVal, maxVal, nSplits)
|
||||
% partialVals = successiveApproxADC_Partial(x, nBits, minVal, maxVal, nSplits)
|
||||
%
|
||||
% Inputs:
|
||||
% x - scalar sensor reading (double) in [minVal, maxVal]
|
||||
% x - scalar sensor reading 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)
|
||||
% minVal - lower bound of sensor range (e.g. -0.25)
|
||||
% maxVal - upper bound of sensor range (e.g. 1.25)
|
||||
% nSplits - how many successive-approximation groups you want (1…nBits)
|
||||
%
|
||||
% Output:
|
||||
% bitGroups - 1×nSplits cell array. Each cell{i} is a 1×groupSize vector
|
||||
% containing the next-most-significant bits (0 or 1).
|
||||
% partialVals - 1×nSplits vector of doubles in [minVal,maxVal].
|
||||
% partialVals(i) corresponds to the analog voltage (or
|
||||
% radians) you'd read after the first i SAR steps,
|
||||
% with all remaining LSBs = 0.
|
||||
%
|
||||
% 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
|
||||
% Method:
|
||||
% 1) Normalize x into [0,1] and scale → integer code ∈ [0,2^nBits–1].
|
||||
% 2) Partition the n bits into nSplits groups.
|
||||
% 3) For each split i:
|
||||
% – Zero out the lower (nBits – bitsUsed) bits to get partial code.
|
||||
% – Convert that code back to a [0,1] fraction.
|
||||
% – Rescale into [minVal,maxVal].
|
||||
%
|
||||
% 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
|
||||
% pVals = successiveApproxADC_Partial(0.5, 10, -0.25, 1.25, 3);
|
||||
% % pVals might be [0.000, 0.500, 0.683] (in radians)
|
||||
|
||||
%%— 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}, ...
|
||||
%— 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);
|
||||
%— 2) Compute full ADC code (integer) from x —
|
||||
normX = (x - minVal) / (maxVal - minVal); % [0…1]
|
||||
rawCode = normX * (2^nBits - 1); % [0…2^nBits-1]
|
||||
code = round(rawCode); % nearest integer
|
||||
code = min(max(code, 0), 2^nBits - 1); % clamp
|
||||
|
||||
% scale to [0…2^nBits-1]
|
||||
rawCode = normX * (2^nBits - 1);
|
||||
uCode = uint64(code); % for bit operations
|
||||
|
||||
% round to nearest integer
|
||||
code = round(rawCode);
|
||||
%— 3) Determine group sizes for the nSplits —
|
||||
baseSize = floor(nBits / nSplits);
|
||||
leftover = mod(nBits, nSplits);
|
||||
groupSizes = [ repmat(baseSize+1, 1, leftover), ...
|
||||
repmat(baseSize, 1, nSplits-leftover) ];
|
||||
|
||||
% 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 );
|
||||
%— 4) Loop over splits, build partial codes, then scale back —
|
||||
partialVals = zeros(1, nSplits);
|
||||
bitsUsed = 0;
|
||||
for i = 1:nSplits
|
||||
bitsUsed = bitsUsed + groupSizes(i); % total MSBs now included
|
||||
dropBits = nBits - bitsUsed; % LSBs to zero
|
||||
|
||||
%%— 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);
|
||||
% a) keep only the top bitsUsed bits:
|
||||
msPart = bitshift(uCode, -dropBits);
|
||||
% b) shift back to full width, zeroing lower bits:
|
||||
pc = bitshift(msPart, dropBits);
|
||||
|
||||
% 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;
|
||||
% c) map pc (∈[0…2^nBits–1]) back to [0,1]:
|
||||
frac = double(pc) / double(2^nBits - 1);
|
||||
% d) rescale into [minVal,maxVal]:
|
||||
partialVals(i) = frac * (maxVal - minVal) + minVal;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
@ -30,8 +30,10 @@ 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);
|
||||
max_time = 1e-2;
|
||||
[ref, t, N] = make_hdd_reference(max_time, Ts_whole_register, 1e-2, 42);
|
||||
|
||||
[ref_third, t_third, N_third] = make_hdd_reference(9*Ts_third_register, Ts_third_register, 1e-2, 42);
|
||||
|
||||
%% Running a Simulation
|
||||
% ADC Delay, no sub-steps - Setting Up Simulation
|
||||
@ -47,9 +49,12 @@ opts.plotting = false;
|
||||
[x_hist, y_hist, u_hist, x_hat_hist] = solve_full_step(opts);
|
||||
|
||||
% ADC delay, with sub-steps
|
||||
opts.K = [0.7+1e-4i 0.7-1e-4i];
|
||||
opts.L = [0.4+0.01i 0.4-0.01i];
|
||||
opts.sys = sys_third_register;
|
||||
opts.sub_steps = 3;
|
||||
opts.r = ref_third;
|
||||
opts.N = N_third/3;
|
||||
opts.res = 12;
|
||||
opts.plotting = true;
|
||||
|
||||
|
||||
@ -11,7 +11,7 @@ function [x_hist, y_hist, u_hist, x_hat_hist] = solve_sub_step(opts)
|
||||
% ▸ L – observer gain matrix
|
||||
% ▸ K – state‑feedback gain matrix
|
||||
% ▸ r – reference trajectory [nu × N]
|
||||
% ▸ sub_step – number of sub_steps per sample
|
||||
% ▸ sub_steps – number of sub_steps per sample
|
||||
% ▸ res – number of bits of resolution
|
||||
|
||||
|
||||
@ -42,7 +42,7 @@ 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;
|
||||
J = opts.sub_steps;
|
||||
res = opts.res;
|
||||
plotting = opts.plotting;
|
||||
|
||||
@ -58,18 +58,17 @@ Ts = sys.Ts;
|
||||
ny = size(C,1);
|
||||
|
||||
% ----------------‑‑ Pre‑allocate histories -----------------------------
|
||||
x_hist = zeros(nx,(N+1)*J);
|
||||
x_hist = zeros(nx,N*J+1);
|
||||
y_hist = zeros(ny,N*J);
|
||||
u_hist = zeros(nu,N*J);
|
||||
|
||||
x_hist(:,1:1*J) = x0;
|
||||
x_hist(:,1) =x0; % Observer bookkeeping
|
||||
|
||||
% 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 = zeros(nx,N*J+1);
|
||||
x_hat_hist(:,1) = x_hat;
|
||||
else
|
||||
x_hat_hist = [];
|
||||
@ -84,36 +83,62 @@ if ~useFB && isempty(u)
|
||||
end
|
||||
|
||||
% Ensure reference is sized correctly if provided
|
||||
if ~isempty(r) && size(r,2)~=N
|
||||
error('opts.r must have N columns.');
|
||||
if ~isempty(r) && size(r,2)~=J*N
|
||||
error('opts.r must have J*N columns.');
|
||||
end
|
||||
|
||||
% ----------------‑‑ Simulation loop ------------------------------------
|
||||
for k = 1:N-1
|
||||
for k = (1:J:(N-1)*J)%N-1
|
||||
k
|
||||
x_hist(:,k+1) = A*x_hist(:,k+0) + B*u_hist(k+0);
|
||||
y_hist(:,k+0) = C*x_hist(:,k+0);
|
||||
|
||||
%ASSUME SENSOR RANGE OF 0-1 RAD -> 0 -> 2^12
|
||||
sensor_value = y_hist(:,k)*2^res;
|
||||
sensor_value = y_hist(:,k+0)*2^res;
|
||||
ADC_output = SAR_ADC_approx(sensor_value, res, -0.2, 1.2, J);
|
||||
|
||||
%FIRST SUBSTEP
|
||||
x_hat =
|
||||
x_hat_hist(:,J*k+1) = A*
|
||||
|
||||
% Compute input
|
||||
u_k = K*(-x_hat_hist(:,k) + (isempty(r) * 0 + ~isempty(r) * r(:,k)));
|
||||
u_hist(k) = u_k;
|
||||
x_hat_hist(:,k+1) = A*x_hat_hist(:,k+0) + ...
|
||||
B*u_hist(k+0) + ...
|
||||
L*( ADC_output(1) - C*x_hat_hist(:,k+0));
|
||||
|
||||
% Plant output
|
||||
y_hist(:,k+1) = C*x_hist(:,k) + D*u_k; %SHIFT RIGHT TO INDUCE DELAY
|
||||
u_hist(k+1) = K*(r(:,k+1)-x_hat_hist(:,k+1));
|
||||
|
||||
% 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;
|
||||
%SECOND SUBSTEP
|
||||
x_hist(:,k+2) = A*x_hist(:,k+1) + B*u_hist(k+1);
|
||||
y_hist(:,k+1) = C*x_hist(:,k+1);
|
||||
|
||||
%Calculate Next State
|
||||
x_hist(:,k+1) = A*x_hist(:,k) + B*u_k;
|
||||
x_hat_hist(:,k+2) = A*(A*x_hat_hist(:,k+0) + ...
|
||||
B*u_hist(k+0) + ...
|
||||
L*(ADC_output(2) - C*x_hat_hist(:,k+0))...
|
||||
) + ...
|
||||
B*u_hist(k+1);
|
||||
|
||||
u_hist(k+2) = K*(r(:,k+2)-x_hat_hist(:,k+2));
|
||||
|
||||
%THIRD SUBSTEP
|
||||
x_hist(:,k+3) = A*x_hist(:,k+2) + B*u_hist(k+2);
|
||||
y_hist(:,k+2) = C*x_hist(:,k+2);
|
||||
|
||||
x_hat_hist(:,k+3) = A*(A*(A*x_hat_hist(:,k+0) + ...
|
||||
B*u_hist(k+0) + ...
|
||||
L*(ADC_output(3) - C*x_hat_hist(:,k+0))...
|
||||
) + ...
|
||||
B*u_hist(k+1)...
|
||||
) + ...
|
||||
B*u_hist(k+2);
|
||||
|
||||
u_hist(k+3) = K*(r(:,k+3)-x_hat_hist(:,k+3));
|
||||
|
||||
end
|
||||
|
||||
x_hist
|
||||
x_hat_hist
|
||||
y_hist
|
||||
u_hist
|
||||
|
||||
|
||||
% ----------------‑‑ Plot results ---------------------------------------
|
||||
|
||||
if plotting
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user