## Omnidirectional Modeling API of Octave by Adnr function obj = omnidirectional( %% viscous friction coefficient related to v %B v (N/m/s) cBv%0.94 %% viscous friction coefficient related to v n %B vn (N/m/s) , cBvn%0.96 %% viscous friction coefficient related to ω %B ω (N/rad/s) , cBw%0.01 %% coulomb friction coefficient related to v %C v (N ) , cCv%2.2 %% coulomb friction coefficient related to v n %C vn (N ) , cCvn%1.5 %% coulomb friction coefficient related to ω %C ω (N.m) , cCw%0.099 %% radius of the robot %b(m) , cb%0.1 %% mass of the robot %M (kg) , cM%1.5 %% inertia moment of the robot %I n (kg.m 2 ) , cIn%0.025 %% angle %δ , cdlt%30 %% radius of the wheels %r 1 , r 2 , r 3 (m) , cri%0.035 %% reduction of the motors %l 1 , l 2 , l 3 , cli%19 %% motor’s armature inductance %L a 1...3 (H) , cLi%0.00011 %% motor’s armature resistance %R a 1...3 (Ω) , cRai%1.69 %% motor’s emf constant %K v 1...3 (V olts/rad/s) , cKv%0.0059 %% motor’s torque constant %K t 1...3 (N.m/A) , cKt%0.0059 %%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Criterion of canonical second-orde system , zeta , omega %% Input constrain , InCon , path_file ) pkg load control obj = struct(); %%%%%%% Origin builder obj.origin = struct(); obj.origin.A = [-((3*(cli^2)*(cKt^2))/(2*cM*cRai*(cri^2)))-(cBv/cM),0,0; 0, -((3*(cli^2)*(cKt^2))/(2*cM*cRai*(cri^2)))-(cBvn/cM),0; 0,0,-((3*(cb^2)*(cli^2)*(cKt^2))/(2*cIn*cRai*(cri^2)))-(cBw/cIn)]; obj.origin.B = [0, (cos(cdlt*pi/180)/cM), -(cos(cdlt*pi/180)/cM); -1/cM, sin(cdlt*pi/180)/cM, sin(cdlt*pi/180)/cM; cb/cIn, cb/cIn, cb/cIn]; obj.origin.K = [-cCv/cM, 0,0; 0, -cCvn/cM, 0; 0,0,-cCw/cM]; obj.LENGTH_STATE = size(obj.origin.A,1); obj.LENGTH_INTPUT = size(obj.origin.B,1); Q = [500 0 0; 0 500 0; 0 0 5;] R = eye(3)*0.3; kst_ = lqr(obj.origin.A,obj.origin.B,Q,R); nst_ = -pinv(eye(size(obj.origin.A))*inv(obj.origin.A-obj.origin.B*kst_)*obj.origin.B); syscl = ss(obj.origin.A-obj.origin.B*kst_, obj.origin.B*nst_, eye(3),0); % step(syscl); obj.origin.Kst = kst_; obj.origin.Nst = nst_; obj.origin.syscl = syscl %%%%%%% A = obj.origin.A; B = obj.origin.B; K = obj.origin.K; obj.LENGTH_STATE = size(A,1)+3; obj.LENGTH_INPUT = size(B,1); obj.A = [zeros(size(A)),eye(size(A));zeros(size(A)),A;]; obj.B = [zeros(size(B));B;]; obj.K = [zeros(size(B,2),size(B,2));K]; obj.getState = @(o) zeros(size(A,1)+3,1); obj.getInput = @(o) zeros(size(B,2),1); obj.InCon = ones(obj.LENGTH_INPUT)*InCon; %%%%%%%%% Make Controller obj.controllabilityTest = @(a,b) rank(ctrb(a,b)); obj.observabilityTest = @(a,c) rank(obsv(a,c)); s = tf('s'); obj.Gref = omega^2/(s^2+(2*zeta*omega)*s+omega^2); p = pole(obj.Gref); Kst = zeros(3,6); Nst = zeros(3,6); Q = { [ 100 0; 0 10;], %x [ 100 0; 0 10;], %y [2000 0; 0 10;] %theta }; R = eye(3)*1; for i = [1:3] A_ = [0 1; 0 A(i,i)]; B_ = [0 0 0; B(i,1) B(i,2) B(i,3)]; kst_ = lqr(A_,B_,Q{i},R); nst_ = -pinv(eye(size(A_))*inv(A_-B_*kst_)*B_); % syscl = ss(A_-B_*kst_, B_*nst_, eye(2),0); % uncomment for debugging % step(syscl); j = ((i-1)*2); Kst(:,1+j:2+j) = kst_; Nst(:,1+j:2+j) = nst_; endfor obj.Kst = [Kst(:,1) Kst(:,3) Kst(:,5) Kst(:,2) Kst(:,4) Kst(:,6)]; obj.Nst = [Nst(:,1) Nst(:,3) Nst(:,5) Nst(:,2) Nst(:,4) Nst(:,6)]; obj.syscl = ss(obj.A-obj.B*obj.Kst,obj.B*obj.Nst,eye(size(obj.A)),[0]); %%%%%%%%% Generate discrete state space function % sgn function, in octave comparison (x compare value) return integer % result of comparison used to address to value in matrix obj.sgn = @(x) [0,1,-1](4-(1*(x<0))-(2*(x>0))-(3*(x==0))); obj.origin.sgn = obj.sgn; obj.limit_motor = @(x) [x,6,-6](4-(1*(x<-6))-(2*(x>6))-(3*(x<6 & x>-6))); obj.origin.limit_motor = obj.limit_motor; % Kinematic function for coordinate robot to global obj.rotationOrtogonal = @(the) [cos(the) sin(the) 0; -sin(the) cos(the) 0; 0 0 1]; % https://en.wikibooks.org/wiki/Control_Systems/State-Space_Equations#Discretization % x[k+1] = (1 + AT)x[k] + TBu[k] + TKsgn(x[k]) % Not using function from control package becouse the system have K term obj.Aterm = @(o,T,st) (eye(o.LENGTH_STATE,o.LENGTH_STATE)+(o.A*T))*st; obj.Bterm = @(o,T,st,in) (T*o.B)*in; obj.Kterm = @(o,T,st,in) ((T*o.K)*[o.sgn(st(4));o.sgn(st(5));o.sgn(st(6))]); % Original state space robot obj.disDotState = @(o,T,st,in) o.Aterm(o,T,st)+o.Bterm(o,T,st,in)+o.Kterm(o,T,st,in); obj.Uterm = @(o,st,ref) (o.Nst*ref - o.Kst*st); obj.Cons = @(u) [u,InCon,-InCon](1+(2*(u<-InCon)+(1*(u>InCon)))); obj.UCons = @(o,u) [o.Cons(u(1)); o.Cons(u(2)); o.Cons(u(3))]; obj.UIn = @(o,st,ref) o.UCons(o,o.Uterm(o,st,ref)); % State space included controller obj.disDotStateCl = @(o,T,st,ref) o.disDotState(o,T,st,o.UCons(o,o.Uterm(o,st,ref))); obj.Y = @(o,st) [o.rotationOrtogonal(st(3))', zeros(size(A))]*st; % Time eq obj.disTime = @(T,ti) (ti + abs(T)); obj.testStabilityExplicit = @(h) abs(eye(size(A))+(h*(A+K))); obj.testStabilityImplicit = @(h) abs(eye(size(A))-(h*(A+K))); %%%%%%%%% PRINT the matrix to file used matrix in python app csvwrite(strcat(path_file,'A.matrix'),obj.A); csvwrite(strcat(path_file,'B.matrix'),obj.B); csvwrite(strcat(path_file,'C.matrix'),[eye(size(A)), zeros(size(A))]); csvwrite(strcat(path_file,'K.matrix'),obj.K); csvwrite(strcat(path_file,'Kst.matrix'),obj.Kst); csvwrite(strcat(path_file,'Nst.matrix'),obj.Nst); [y,t] = step(obj.Gref); csvwrite(strcat(path_file,'Gref.csv'),[y,t]); csvwrite(strcat(path_file,'poleGref.csv'),pole(obj.Gref)); endfunction