tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Align the centers of rotation of Ry, Hexa, NASS with the sample

Browse Source
This commit is contained in:
Thomas Dehaeze
2018-10-30 14:37:19 +01:00
parent ef6f4613b1
commit 389fd66ba4
27 changed files with 90 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/generateDiagPidControl.m
View File

@@ -7,7 +7,7 @@ function [K] = generateDiagPidControl(G, fs)
%%
K = tf(zeros(6));
for i = 1:5
for i = 1:6
input_name = G.InputName(i);
output_name = G.OutputName(i);
K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));

6
src/identifyPlant.m
View File

@@ -24,6 +24,7 @@ function [sys] = identifyPlant(opts_param)
io(5) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
io(6) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
io(7) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
io(8) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
%% Run the linearization
G = linearize(mdl, io, 0);
@@ -33,7 +34,8 @@ function [sys] = identifyPlant(opts_param)
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'};
%% Create the sub transfer functions
% From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
@@ -46,4 +48,6 @@ function [sys] = identifyPlant(opts_param)
sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
% From forces applied on NASS's legs to displacement of each leg
sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
% From forces applied on NASS's legs to displacement of each leg
sys.G_plant = minreal(G({'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
end

8
src/runSimulation.m
View File

@@ -3,14 +3,14 @@ function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
K_obj = load('./mat/K_fb.mat');
K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
K_obj = load('./mat/K_fb_iff.mat');
K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
elseif strcmp(ctrl_type, 'ol')
K = tf(zeros(6)); %#ok
save('./mat/controller.mat', 'K');
save('./mat/controllers.mat', 'K');
else
error('ctrl_type should be cl or ol');
end
@@ -46,7 +46,7 @@ function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
sim('sim_nano_station_ctrl.slx');
%% Split the Dsample matrix into vectors
[Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Dsample.Data, 1); %#ok
[Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok
time = Dsample.Time; %#ok
%% Save the result