63 lines
3.1 KiB
Matlab
63 lines
3.1 KiB
Matlab
function [nano_hexapod] = initializeNanoHexapod(args)
|
|
arguments
|
|
% initializeFramesPositions
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 175e-3
|
|
% generateGeneralConfiguration
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e-3;
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
|
% initializeStrutDynamics
|
|
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
|
% initializeCylindricalPlatforms
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
% initializeCylindricalStruts
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
% inverseKinematics
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
stewart = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
|
|
|
stewart = generateGeneralConfiguration(stewart, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
|
|
|
|
stewart = computeJointsPose(stewart);
|
|
|
|
if strcmp(args.actuator, 'piezo')
|
|
stewart = initializeStrutDynamics(stewart, 'Ki', 1e7*ones(6,1), ...
|
|
'Ci', 1e2*ones(6,1));
|
|
elseif strcmp(args.actuator, 'lorentz')
|
|
stewart = initializeStrutDynamics(stewart, 'Ki', 1e4*ones(6,1), ...
|
|
'Ci', 1e2*ones(6,1));
|
|
else
|
|
error('args.actuator should be piezo or lorentz');
|
|
end
|
|
|
|
stewart = initializeCylindricalPlatforms(stewart, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
|
|
|
stewart = initializeCylindricalStruts(stewart, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
|
|
|
stewart = computeJacobian(stewart);
|
|
|
|
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
|
|
|
nano_hexapod = stewart;
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
|
end
|