2021-03-02 08:24:32 +07:00
|
|
|
function obj = formationControl1stModelBuilder(param)
|
2021-01-26 12:23:22 +07:00
|
|
|
addpath("./networks-toolbox");
|
2021-03-02 08:24:32 +07:00
|
|
|
edgeL = param.edgeL;
|
|
|
|
|
model = param.model.origin;
|
|
|
|
|
kp1 = param.kp1;
|
|
|
|
|
kp2 = param.kp2;
|
|
|
|
|
ki1 = param.ki1;
|
|
|
|
|
ki2 = param.ki2;
|
|
|
|
|
dScale = param.dScale;
|
|
|
|
|
nNodeVref = param.nNodeVref;
|
2021-01-26 12:23:22 +07:00
|
|
|
obj = struct();
|
|
|
|
|
robStateLen = size(model.A,1)
|
|
|
|
|
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
|
|
|
|
|
obj.m = length(obj.d);
|
|
|
|
|
obj.n = numNodes(edgeL);
|
|
|
|
|
obj.nodeStateLength = robStateLen*obj.n;
|
|
|
|
|
obj.stateLength = 2 * obj.nodeStateLength + obj.m;
|
|
|
|
|
_nst = kron(eye(obj.n), model.Nst);
|
|
|
|
|
_kst = kron([zeros(obj.n) eye(obj.n)], model.Kst);
|
|
|
|
|
%obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) eye(obj.n)],model.A) + kron([zeros(obj.n) eye(obj.n);zeros(obj.n) zeros(obj.n)],eye(obj.n));
|
|
|
|
|
% pilih robot mana yang ingin dijalankan dengan mengganti 0 menjadi 1
|
|
|
|
|
% r 1 2 3 r 1 2 3
|
|
|
|
|
% v v v v v v
|
2021-03-02 08:24:32 +07:00
|
|
|
obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) diag([1 0 0])],model.A) + kron([zeros(obj.n) diag([1 0 0]);zeros(obj.n) zeros(obj.n)],eye(obj.n));
|
2021-01-26 12:23:22 +07:00
|
|
|
obj.B = kron([zeros(obj.n); eye(obj.n)],model.B);
|
|
|
|
|
obj.Ksgn = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) eye(obj.n)],model.K);
|
|
|
|
|
obj.controller = @(set_point,st) arrayfun(model.limit_motor,_nst * set_point + _kst * st(1:(2 * obj.nodeStateLength)));
|
|
|
|
|
obj.formation_controller = @(st) -obj.R(st(1:obj.nodeStateLength),obj.K)'*kp1*(obj.R(st(1:obj.nodeStateLength),obj.K)*st(1:obj.nodeStateLength)-(obj.d*dScale)) -obj.R(st(1:obj.nodeStateLength),obj.K)'*ki1*st(obj.stateLength-obj.m+1:obj.stateLength,1);
|
|
|
|
|
obj.formation_control_dot = @(st) [obj.R(st(1:obj.nodeStateLength),obj.K) zeros(obj.m, obj.nodeStateLength + obj.m)];
|
|
|
|
|
obj.stateA = @(st) [obj.A zeros(obj.stateLength-obj.m, obj.m); obj.formation_control_dot(st)];
|
|
|
|
|
obj.stateB = [obj.B zeros(obj.stateLength-obj.m, obj.m); zeros(obj.m, size(model.B,2)*obj.n) eye(obj.m)];
|
|
|
|
|
obj.stateKsgn = @(st,h) (h*[obj.Ksgn zeros(obj.stateLength-obj.m, obj.m); zeros(obj.m,obj.stateLength)])*arrayfun(model.sgn,st);
|
|
|
|
|
obj.stateInput = @(st) [obj.controller(obj.formation_controller(st), st); obj.d*dScale];
|
|
|
|
|
|
|
|
|
|
obj.ss = @(st,vRef) obj.stateA(st)*st + obj.stateKsgn(st,1) + obj.stateB *obj.stateInput(st);
|
|
|
|
|
|
|
|
|
|
obj.dss = @(st,h,vRef) (eye(size(obj.stateA(st))) + (h*obj.stateA(st)))*st + obj.stateKsgn(st,h) + (h*obj.stateB)*obj.stateInput(st);
|
|
|
|
|
endfunction
|
