Skip to content

Commit

Permalink
Major refactor of SpotGnc from scalar inputs to vector inputs. SpotCo…
Browse files Browse the repository at this point in the history 
…ntroller.m, SpotCtrlErr.m, SpotRefGen.m, ulrich2017.slx: major refactor. Run_Simulation_ETHZ.m: added third simulation and plotting. SpotGnc.m: removal of pure feed-forward control.
  • Loading branch information
@adamvigneron
adamvigneron committed Jun 12, 2024
1 parent 1d3679f commit f982f13
Show file tree
Hide file tree
Showing 6 changed files with 291 additions and 194 deletions.
79 changes: 68 additions & 11 deletions Projects/ulrich2017/Run_Simulation_ETHZ.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,21 +173,78 @@
uVec2 = dataClass.RED_Fx_Sat_N(idxPh3_ILC) / mRED;
yVec2 = dataClass.RED_Px_m(idxPh3_ILC);

uFbk2 = (dataClass.RED_Fx_Kp_N(idxPh3_ILC) + dataClass.RED_Fx_Kd_N(idxPh3_ILC) ) / mRED;
uFwd2 = dataClass.RED_Fx_u0_N(idxPh3_ILC) / mRED;

%% SAVE
%% SIMULATION 3: FURTHER IMPROVEMENTS?

% save the simulation data
myEvent.Value = 'suppressWaitfor';
myApp.public_SaveSimulationDataButtonPushed(myEvent);
% repeat the command update
uTilde = uVec2 - uVec0;
yTilde = yVec2 - yVec0;

dHat = G\yTilde - F*uTilde - G\H*uTilde;
uNew = -F\dHat;

%% SHUTDOWN
fNew = interp1(tVec, mRED*uNew, feedForward.Time(idxPh3), 'previous', 'extrap');
feedForward.Data(idxPh3,SpotCoord.xRed) = fNew;

% close the simulink model; close the figure (and app); clear the workspace
bdclose;
close(myFig);
clear;
% start the simulation
myEvent.Value = 'suppressHardwareWarning';
myApp.public_StartSimulationButtonPushed([]);

% reset figure style
reset(groot);
% extract the relevant data
uVec3 = dataClass.RED_Fx_Sat_N(idxPh3_ILC) / mRED;
yVec3 = dataClass.RED_Px_m(idxPh3_ILC);


%% PLOT

% output signal
figure('name','RED_Px_m (value)')
plot(tVec,[yVec0 yVec1 yVec2 yVec3]);
legend('nominal','1: feedback', '2: feedback + feedfwd', ...
'3: further improvement', 'Location', 'BestOutside');
xlabel('Time\_s');
ylabel('RED\_Px\_m');
title('output signal');

% output error
figure('name','RED_Px_m (error)')
plot(tVec,[yVec0-rVec yVec1-rVec yVec2-rVec yVec3-rVec]);
legend('nominal','1: feedback', '2: feedback + feedfwd', ...
'3: further improvement', 'Location', 'BestOutside');
xlabel('Time\_s');
ylabel('RED\_Px\_m');
title('output error');

% control signal
figure('name','RED_Fx_Sat_N')
plot(tVec,mRED * [uVec0 uVec1 uVec2 uFbk2 uFwd2 uVec3]);
hold on;
plot(disturbFT.Time(idxPh3,SpotCoord.xRed),-1 * disturbFT.Data(idxPh3,SpotCoord.xRed),'k--')
legend('nominal','1: feedback', '2: feedback + feedfwd', ...
'2: feedback', '2: feedfwd', '3: further improvement', ...
[char(8211) '1 \times disturbance'], 'Location', 'BestOutside');
xlabel('Time\_s');
ylabel('RED\_Fx\_Sat\_N');
title('control signal');
ylim([-0.2 0.1]);


% %% SAVE
%
% % save the simulation data
% myEvent.Value = 'suppressWaitfor';
% myApp.public_SaveSimulationDataButtonPushed(myEvent);
%
%
% %% SHUTDOWN
%
% % reset figure style
% reset(groot);
%
% % close the simulink model; close the figure (and app); clear the workspace
% bdclose;
% close(myFig);
% clear;

152 changes: 78 additions & 74 deletions Projects/ulrich2017/SpotController.m
View file
Original file line number Diff line number Diff line change
@@ -1,86 +1,90 @@
function [F,diag] = SpotController(e, phase, coord, feedFwd, paramCtrl)

%% persistent variables

% definition of persistent variables
persistent eOld;
persistent eDeltaOld;

% initialization of persistent variables
if isempty(eOld)
eOld = e;
eDeltaOld = 0;
end

function [F,debug] = SpotController(phase, err, feedFwd, paramCtrl)

%% initialization of diagnostic output
%% initialization of output and persistent variables

numPhase = size(paramCtrl,1); %#ok<NASGU>
numCoord = size(paramCtrl,2);
numDiag = 3;
coords = enumeration('SpotCoord');
numCoord = length(coords);
numDebug = 3;

diag = zeros(numDiag,numCoord);


%% select a control method

myFun = paramCtrl(phase,coord).fun;
% output variables
F = zeros(numCoord,1);
debug = zeros(numDebug,numCoord);

switch myFun

case SpotGnc.ctrlNone

F = 0;

case SpotGnc.ctrlPd
k1 = paramCtrl(phase,coord).k1; % Kp
k2 = paramCtrl(phase,coord).k2; % Kd
k3 = paramCtrl(phase,coord).k3; % baseRate

eDelta = e - eOld;

if eDelta == 0
eDelta = eDeltaOld;
end

% F = Kp*e + Kd*(de/dt)
F = k1*e + k2*eDelta/k3;

diag(1,coord) = k1*e;
diag(2,coord) = k2*eDelta/k3;

eOld = e;
eDeltaOld = eDelta;

case SpotGnc.ctrlPdFwd
k1 = paramCtrl(phase,coord).k1; % Kp
k2 = paramCtrl(phase,coord).k2; % Kd
k3 = paramCtrl(phase,coord).k3; % baseRate
k4 = paramCtrl(phase,coord).k4; % beta

eDelta = e - eOld;

if eDelta == 0
eDelta = eDeltaOld;
end

% F = Kp*e + Kd*(de/dt) + beta*uOld
F = k1*e + k2*eDelta/k3 + k4*feedFwd(coord);
% persistent variables - definition
persistent errOld;
persistent errDeltaOld;

% persistent variables - initialization
if isempty(errOld)
errOld = err;
errDeltaOld = zeros(numCoord,1);
end

diag(1,coord) = k1*e;
diag(2,coord) = k2*eDelta/k3;
diag(3,coord) = k4*feedFwd(coord);

eOld = e;
eDeltaOld = eDelta;
%% loop over all coordinates

case SpotGnc.ctrlFwd
F = feedFwd(coord);
for k = 1:numCoord

coord = coords(k);

otherwise
error('SpotController.m:\n function SpotGnc(%d) not defined for SpotPhase(%d) and SpotCoord(%d).\n\n', int32(myFun), int32(phase), int32(coord))
%% select a control method

myFun = paramCtrl(phase,coord).fun;

switch myFun

case SpotGnc.ctrlNone

F(coord) = 0;

case SpotGnc.ctrlPd
k1 = paramCtrl(phase,coord).k1; % Kp
k2 = paramCtrl(phase,coord).k2; % Kd
k3 = paramCtrl(phase,coord).k3; % baseRate

eDelta = err(coord) - errOld(coord);

if eDelta == 0
eDelta = errDeltaOld(coord);
end

% F = Kp*e + Kd*(de/dt)
F(coord) = k1*err(coord) + k2*eDelta/k3;

debug(1,coord) = k1*err(coord);
debug(2,coord) = k2*eDelta/k3;

errOld(coord) = err(coord);
errDeltaOld(coord) = eDelta;

case SpotGnc.ctrlPdFwd
k1 = paramCtrl(phase,coord).k1; % Kp
k2 = paramCtrl(phase,coord).k2; % Kd
k3 = paramCtrl(phase,coord).k3; % baseRate
k4 = paramCtrl(phase,coord).k4; % beta

eDelta = err(coord) - errOld(coord);

if eDelta == 0
eDelta = errDeltaOld(coord);
end

% F = Kp*e + Kd*(de/dt) + beta*uOld
F(coord) = k1*err(coord) + k2*eDelta/k3 + k4*feedFwd(coord);

debug(1,coord) = k1*err(coord);
debug(2,coord) = k2*eDelta/k3;
debug(3,coord) = k4*feedFwd(coord);

errOld(coord) = err(coord);
errDeltaOld(coord) = eDelta;

otherwise
error('SpotController.m:\n function SpotGnc(%d) not defined for SpotPhase(%d) and SpotCoord(%d).\n\n', int32(myFun), int32(phase), int32(coord))

end % switch myFun

end % switch myFun
end % loop coords

end % function

70 changes: 45 additions & 25 deletions Projects/ulrich2017/SpotCtrlErr.m
View file
Original file line number Diff line number Diff line change
@@ -1,38 +1,58 @@
function err = SpotCtrlErr(ref, act, phase, coord, paramCtrlErr)
function err = SpotCtrlErr(phase, est, ref, paramCtrlErr)

myFun = paramCtrlErr(phase,coord).fun;
%% initialization of output variables

switch myFun
coords = enumeration('SpotCoord');
numCoord = length(coords);

case SpotGnc.errMinus

err = ref - act;

case SpotGnc.errMinusWrap

errWrap = wrapToPi(ref) - wrapToPi(act);

if abs(errWrap) < pi
err = errWrap;
else
err = errWrap - sign(errWrap)*2*pi;
end
err = zeros(numCoord,1);


%% loop over all coordinates

for k = 1:numCoord

coord = coords(k);

case SpotGnc.errHough
%% select an error calculation method
myFun = paramCtrlErr(phase,coord).fun;

switch myFun

case SpotGnc.errMinus

err(coord) = ref(coord) - est(coord);

case SpotGnc.errMinusWrap

errWrap = wrapToPi( ref(coord) ) - wrapToPi( est(coord) );

xRef = [cos(ref) sin(ref); -sin(ref) cos(ref)]*[1;0];
yRef = [cos(ref) sin(ref); -sin(ref) cos(ref)]*[0;1];
if abs(errWrap) < pi
err(coord) = errWrap;
else
err(coord) = errWrap - sign(errWrap)*2*pi;
end

xAct = [cos(act) sin(act); -sin(act) cos(act)]*[1;0];

errX = xRef - xAct;
err = -sign(yRef'*errX) * norm(errX);
case SpotGnc.errHough

r = ref(coord);
e = est(coord);

xRef = [cos(r) sin(r); -sin(r) cos(r)]*[1;0];
yRef = [cos(r) sin(r); -sin(r) cos(r)]*[0;1];

xAct = [cos(e) sin(e); -sin(e) cos(e)]*[1;0];

errX = xRef - xAct;
err(coord) = -sign(yRef'*errX) * norm(errX);

otherwise
error('SpotCtrlErr.m:\n function SpotGnc(%d) not defined for SpotPhase(%d) and SpotCoord(%d).\n\n', int32(myFun), int32(phase), int32(coord))

otherwise
error('SpotCtrlErr.m:\n function SpotGnc(%d) not defined for SpotPhase(%d) and SpotCoord(%d).\n\n', int32(myFun), int32(phase), int32(coord))
end % switch myFun

end % switch myFun
end % loop coords

end % function

1 change: 0 additions & 1 deletion Projects/ulrich2017/SpotGnc.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@
ctrlNone(201)
ctrlPd(202)
ctrlPdFwd(203)
ctrlFwd(204)
%
errMinus(301)
errMinusWrap(302)
Expand Down
Loading

0 comments on commit f982f13

Please sign in to comment.