Battery

.gitignore

@@ -1,11 +1,24 @@
 # gitignore file
-*
+fig/
+Latex/
+results/
+workspace/
+config/
+x86/
+doc/
+slprj/
+jackal/
 
-# exceptions (dir)
-!data/
-!doc/
-!Lib/
+# files
+*.asv
+*.save
+.gitignore
+*.autosave
+*.original
+*.slxc
+*.zip
+*.prj
+*.tmp
 
-#exceptions (files)
-!COPYING
-!README.md
+# exceptions
+!results/simulations/battery/report

Lib/control/control.m

@@ -1,26 +1,91 @@
-%% CONTROL
-% file: control.m
-% author: Federico Oliva
-% date: 10/01/2022
-% description: this function implements the control law
-% INPUT:
-% t: time instant
-% drive: error variable
-% params: structure with all the necessary parameters 
-% OUTPUT:
-% u: control variable
-function u = control(t,drive,params)
-
-    % check if the input is enabled
+function u = control(t,drive,params,obs)
+
+    % init input
+    u = zeros(params.dim_input,length(t));
+
     if params.input_enable
+
+        % traj
+        traj = obs.init.traj;
+
+        % PWM 3 levels
+
+        % 2nd order system
+%         u(1,:) = -2*params.rhox(1)*params.wnx(obs.init.traj)*drive(2) -params.wnx(obs.init.traj)^2*drive(1);
+%         u(2,:) = -2*params.rhoy(1)*params.wny(obs.init.traj)*drive(6) -params.wny(obs.init.traj)^2*drive(5);
+
+        % sum of sines (position)
+%         target(1,:) = exp(-params.rhox.*t).*(params.Ax(1)*sin(params.wnx(1).*t + params.phi(1)) + params.Ax(2)*sin(params.wnx(2).*t + params.phi(1)));
+%         target(2,:) = exp(-params.rhoy.*t).*(params.Ay(1)*sin(params.wny(1).*t + params.phi(2)) + params.Ay(2)*sin(params.wny(2).*t + params.phi(2)));
+%         u(1,:) = params.Ku(1)*(target(1,:)-drive(1,:));
+%         u(2,:) = params.Ku(2)*(target(2,:)-drive(6,:));
 
-    % !REMARK!
-    % simply uncomment or add the law you need, according to params
 
-    %%%% control law - battery %%%%
-    u = [sin(t); zeros(size(t))];
+        % patrolling on x-y
+        T = t(1);
+        vx = 0;
+        vy = 0;
+        if mod(T,params.freq_u) < 0.25*params.freq_u
+            vx = params.amp_ux;
+        elseif mod(T,params.freq_u) < 0.5*params.freq_u
+            vy = params.amp_uy;
+        elseif mod(T,params.freq_u) < 0.75*params.freq_u
+            vx = -params.amp_ux;
+        else
+            vy = -params.amp_uy;
+        end        
+        u(1,:) = params.Ku(1)*(vx-drive(params.pos_v(1)));
+        u(2,:) = params.Ku(2)*(vy-drive(params.pos_v(2)));
 
-    else
-        u = zeros(params.dim_input,1);
+        % compute the time index
+        for i=1:length(t)
+            tdiff = obs.setup.time-t(i);   
+            pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+            pos(i) = max(1,pos(i));        
+        end  
+        ind = pos(1);
+
+        % hills on z
+        p_now = drive(params.pos_p(1:2));
+        p_est = zeros(1,2);
+        p_grid = [params.X_gauss(1,:); params.Y_gauss(:,1)'];
+        for i=1:2
+            pdiff = p_grid(i,:)-p_now(i);   
+            p_est(i) = find(abs(pdiff) == min(abs(pdiff)),1,'first');                
+        end
+        z_des = params.G_gauss(p_est(2),p_est(1));
+        z_now = drive(params.pos_p(3));
+        zdot_now = drive(params.pos_v(3));
+
+        obs.init.params.z_des_story(:,ind) = z_des;
+        obs.init.params.z_now_story(:,ind) = z_now;
+
+        % error        
+        e = (z_des-z_now);
+        obs.init.params.err(traj).val(:,ind) = e;
+
+        % derivative        
+        [edot, obs.init.params.err_der_buffer, obs.init.params.err_der_counter(traj).val] = PseudoDer(params.Ts,...
+            obs.init.params.err(traj).val(:,max(1,ind-1)),params.wlen_err,...
+            params.buflen_err,params.dim_err,0,0,obs,obs.init.params.err_der_buffer,obs.init.params.err_der_counter(traj).val);
+        obs.init.params.err_der(traj).val(:,ind) = edot;
+
+        % integral
+        obs.init.params.err_int(traj).val(:,ind) = obs.init.params.err_int(traj).val(:,max(1,ind-1)) + e*params.Ts;
+        eint = obs.init.params.err_int(traj).val(:,ind);
+
+        % control
+        u(3,:) = -params.Kz(1)*-e -params.Kz(2)*zdot_now + params.Kff*[e edot eint]';
+
+        % ony for testing
+        u(4,:) = z_des;
+
+        % Volterra Lotka
+%         u(1,:) = params.K1*(drive(1)-params.target(1));
+%         u(2,:) = params.K2*(drive(1)-params.target(1));
+%         % sat
+%         u(1,:) = min(max(-params.umax,u(1,:)),params.umax);
+%         u(2,:) = min(max(-params.umax,u(2,:)),params.umax);
     end
+
 end

Lib/control/control_TCV.m

@@ -0,0 +1,28 @@
+%% CONTROL
+% file: control.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the control law
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with all the necessary parameters 
+% OUTPUT:
+% u: control variable
+function u = control_TCV(t,drive,params,obs)
+
+    % check if the input is enabled
+    if params.input_enable
+
+        % !REMARK!
+        % simply uncomment or add the law you need, according to params
+
+%         for i=1:params.m
+%             tmpstr =  ['u(', num2str(i) ',:) = ', num2str(i), '*sin(', num2str(i), '*t);'];
+%             eval(tmpstr); 
+%         end            
+        u(:,1) = ones(1,length(t));
+    else
+        u = zeros(params.dim_input,1);
+    end
+end

Lib/control/control_battery.m

@@ -0,0 +1,33 @@
+%% CONTROL
+% file: control.m
+% author: Federico Oliva
+% date: 10/01/2022
+% description: this function implements the control law
+% INPUT:
+% t: time instant
+% drive: error variable
+% params: structure with all the necessary parameters 
+% OUTPUT:
+% u: control variable
+function u = control_battery(t,drive,params,obs)
+
+    % init input
+    u = zeros(1,length(t));
+    % params.dim_input
+
+    % compute the time index
+    for i=1:length(t)
+        tdiff = obs.setup.time-t(i);   
+        pos(i) = find(abs(tdiff) == min(abs(tdiff)),1,'first');    
+        pos(i) = max(1,pos(i));        
+    end 
+
+    % check if the input is enabled
+    if params.input_enable
+
+        % from input data        
+        u = params.u_sim(:,pos);
+
+
+    end
+end

Lib/control/drive.m

@@ -0,0 +1,78 @@
+%% control drive function (observer or control design)
+function drive_out = drive(varargin) 
+
+    obj = varargin{1};   
+
+    % init drive
+    drive_out = [];
+
+    % x - varargin
+    x_star = varargin{2}(1:obj.init.params.dim_state,:);            
+    x = varargin{3}(1:obj.init.params.dim_state,:);      
+
+
+    % just x
+%     drive_out = [drive_out; x(obj.setup.plot_vars)];
+    drive_out = [drive_out; x];
+
+    % get y    
+    if 1    
+    y_meas = varargin{4}(:,:,end);
+    pos = varargin{5};
+
+    y = obj.setup.measure(x,obj.init.params,pos,obj.init.input_story(obj.init.traj).val(:,max(1,pos-1)));    
+
+    %%% compute filters %%%
+    y_filt = [];
+    y_filt_meas = [];
+    if 1 && obj.setup.Nfilt > 0                     
+
+        % how long iterate        
+        tspan_pos = [max(1,pos-1), pos];                
+
+        % update buffer - only first (RK4)
+        obj.init.Y_buffer_control(obj.init.traj).val(1,:,pos) = y;                        
+
+        try
+            ok = 1;
+            [Y_filt, x_filter] = obj.measure_filter(obj.init.Y_buffer_control(obj.init.traj).val,tspan_pos,obj.init.X_filter_buffer_control(obj.init.traj));
+        catch            
+            ok=0;
+        end
+
+        if ok
+            for filt=1:obj.setup.Nfilt
+                for dim=1:obj.setup.dim_out
+                    % Lsim
+                    y_filt(filt,dim) = Y_filt{filt,dim}.val(end);
+                    obj.init.X_filter_buffer_control(obj.init.traj).val{filt,dim}(:,unique(tspan_pos)) = x_filter{filt,dim}.val;
+
+                end
+
+                % update buffer - only first (RK4)
+                obj.init.Y_buffer_control(obj.init.traj).val(1+filt,:,pos) = y_filt(filt,:);
+            end
+
+            y_filt = reshape(y_filt,size(y));
+            y_pad = zeros(size(y).*[2,1]);
+            y_pad(1:2:end) = y;
+            y_pad(2:2:end) = y_filt;
+            y = y_pad;
+
+        else
+%             y_meas = reshape(y_meas,size(y_meas,1)*size(y_meas,2));
+            y = y_meas;            
+        end
+    end
+
+        y_meas = reshape(y_meas,size(y));
+        tmp = y_meas-y;        
+        tmp = reshape(tmp,length(y),1);
+
+        drive_out = [drive_out; tmp];
+    end
+
+    % save drive story
+    obj.init.drive_out(obj.init.traj).val(:,pos) = drive_out;
+
+end

Lib/control/generate_HCCP.m

@@ -0,0 +1,18 @@
+%% 
+function [HCCP, tspan, tspan_pos] = generate_HCCP(Ts,Cnh)
+
+    % time span
+    tspan = 0:Ts:60;
+    tspan_pos = 1:length(tspan);
+
+    % intervals
+    tstops = [tspan_pos(1) find(tspan==20) find(tspan==50) tspan_pos(end)];
+    tamplitudes = [1*Cnh 0 -0.5*Cnh];
+
+    % generate wave
+    HCCP = zeros(size(tspan));
+    for i=1:length(tamplitudes)
+       HCCP(tstops(i):tstops(i+1)) = tamplitudes(i); 
+    end
+
+end

Lib/integration/odeDD.m

@@ -11,6 +11,7 @@
 function out = odeDD(varargin)
 
 % init params
+nargin = length(varargin);
 
 % handle function to the dynamics
 ode = varargin{1};
@@ -21,35 +22,32 @@
 % y0: initial condition
 x0 = varargin{3};
 
-% U: input
-U = varargin{4};
-
-% TF: transfer function
-TF = varargin{5};
+% integrations options
+if nargin > 3
+    opts = varargin{4};
+end
 
 % number of state's components
 N = length(x0);           
 
 % numer of time steps
-M = length(tspan);   
+M = length(tspan);
 
 % initial time instant
 t0 = tspan(1);
 
 % Matrices allocation
 X = zeros(N,M);
 X(:,1) = x0;
-Y(:,1) = 0*U(:,1);
 
 % integration
 for i = 1:M-1
 
     % DD step
-    [Y(:,i+1),X(:,i+1)] = feval(ode,tspan(i),X(:,i),U,TF.A, TF.B, TF.C, TF.D);   
+    X(:,i+1) = feval(ode,tspan(i),X(:,i));   
 
 end
 
 % store
-out.x = X;
-out.y = Y(:,2:end);
+out.y = X;
 end