diff --git a/sw/simulator/scilab/q3d/poly_utils.sci b/sw/simulator/scilab/q3d/poly_utils.sci deleted file mode 100644 index 8098516867..0000000000 --- a/sw/simulator/scilab/q3d/poly_utils.sci +++ /dev/null @@ -1,98 +0,0 @@ -function P = coeff_from_bound(a,b,d_time) - - d = length(a)-1; - N = 2*d+1; - if d+1~=length(b) - error('coeff_from_bound:'... - +' boundary conditions not compatible'); - else - P = list(); - A = lin_sys(d); - for i = 1:d+1 - a(i) = d_time^(i-1)*(-1)^(N-i+1)*a(i); - b(i) = d_time^(i-1)*b(i); - end - p_0_init = (A\a)'; - p_0_end = (A\b)'; - p_0_end = p_0_end(:,$:-1:1); - P($+1) = cat(2,p_0_init,p_0_end); - for i = 1:d - P($+1) = deriv_coeff(P($),i,N); - end - for i = 1:d - P(i+1) = d_time^(-i)*P(i+1); - end - end - -endfunction - -function p_d = deriv_coeff(p,d,n) - - for i = 1:length(p)-1 - p_d(i) = i*p(i+1)+(n-(d+i)+2)*p(i); - end - -endfunction - -function res = arr(n,m) - - if m>n - error('arr: could not compute arrangement') - else - res = factorial(n)/factorial(n-m); - end - -endfunction - -function M = lin_sys(d) - - n = 2*d+1; - for i = 1:d+1 - for j = 1:d+1 - if j<=i - M(i,j) = arr(i-1,j-1)*arr(n-j+1,i-j); - else - M(i,j) = 0; - end - end - end - -endfunction - -function res = polyval(p,t,t0,tf) - - // Coordinate change - u = (t-t0)/(tf-t0); - - // Normalized Polynomial Value - res = 0; - n = length(p) - for i = 1:n - res = res + p(i)*u^(i-1)*(u-1)^(n-i); - end - -endfunction - -function present(P) - - d = length(P); - t = linspace(0,1,50); - for i = 1:length(t) - for j = 1:d - RES(j,i) = polyval(P(j),t(i)); - end - end - - clf(); - drawlater(); - for i = 1:d - subplot(d,1,i) - plot2d(t,RES(i,:),2); - xgrid(); - end - drawnow(); - -endfunction - - - diff --git a/sw/simulator/scilab/q3d/test_12.sce b/sw/simulator/scilab/q3d/test_12.sce deleted file mode 100644 index 9b39ea5b14..0000000000 --- a/sw/simulator/scilab/q3d/test_12.sce +++ /dev/null @@ -1,86 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_utils.sci'); -exec('q3d_fdm.sci'); -exec('q3d_ctl.sci'); -exec('q3d_ref_misc.sci'); -exec('poly_utils.sci'); - - -start = [ -10; 0 - 0; 0 - 0; 0 - 0; 0 - 0; 0 ]; - -circle_center = [ 0; 0 ]; - - -stop = [ 200; 0 - 0; 0 - 0; 0 - 0; 0 - 0; 0 ]; - -if 0 - time_ref = [0]; - ref = start; - - // stay - //[time_ref, ref] = get_reference_poly3(time_ref, ref, 1, start); - [time_ref, ref] = get_reference_poly3(time_ref, ref, 20, stop); - //[time_ref, ref] = get_reference_poly3(time_ref, ref, 1, stop); - - - clf(); - ref_display(time_ref, ref); - pause -end - -time_ref = [0]; -ref = start; -[time_ref, ref] = get_reference_lti4(time_ref, ref, 10, stop(1:2)); - -clf(); -ref_display(time_ref, ref); -pause - -fdm_init(time_ref, ref); -global fdm_state - -ctl_init(); - -global ctl_motor; -ctl_motor(:,1) = fdm_mass * fdm_g * [0.5;0.5]; - -global ctl_ref_0; -global ctl_ref_1; -global ctl_ref_2; -global ctl_ref_3; -global ctl_ref_4; -ctl_ref_0 = ref(1:2,:); -ctl_ref_1 = ref(3:4,:); -ctl_ref_2 = ref(5:6,:); -ctl_ref_3 = ref(7:8,:); -ctl_ref_4 = ref(9:10,:); - - -for i=1:length(fdm_time)-1 - - fdm_run(i+1, ctl_motor(:,i)); - ctl_run_flatness(i+1); - -end -if 1 -clf(); -plot2d(fdm_state(FDM_SX,:), fdm_state(FDM_SZ,:),2); -plot2d(ctl_ref_0(AXIS_X,:), ctl_ref_0(AXIS_Z,:),3); - pause -end -clf(); -ctl_display(); - pause - -gen_video(); \ No newline at end of file diff --git a/sw/simulator/scilab/q3d/test_2.sce b/sw/simulator/scilab/q3d/test_2.sce deleted file mode 100644 index 1af69e7b18..0000000000 --- a/sw/simulator/scilab/q3d/test_2.sce +++ /dev/null @@ -1,50 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_utils.sci'); -exec('q3d_fdm.sci'); -exec('q3d_ctl.sci'); - - - -fdm_init(0,17.0); -ctl_init(); - -global ctl_motor; -ctl_motor(:,1) = fdm_mass * fdm_g * [0.5;0.5]; - -for i=1:length(fdm_time)-1 - - fdm_run(i+1, ctl_motor(:,i)); - - if fdm_time(i+1) < 1 - sp_pos= [ 0; 0]; - elseif fdm_time(i+1) < 5 - sp_pos= [ 1; 1]; - elseif fdm_time(i+1) < 9 - sp_pos= [ 0; 1]; - elseif fdm_time(i+1) < 13 - sp_pos= [ 1; 0]; - elseif fdm_time(i+1) < 17 - sp_pos= [ 0; 0]; - end - - ctl_run(i+1, sp_pos); - -end - -//set("current_figure",0); -clf(); -//f=get("current_figure"); -//f.figure_name="CTL"; - -if 0 - drawlater(); - ctl_display(); - drawnow(); - pause -end -if 0 -gen_video(); -end diff --git a/sw/simulator/scilab/q3d/test_3.sce b/sw/simulator/scilab/q3d/test_3.sce deleted file mode 100644 index e40f551106..0000000000 --- a/sw/simulator/scilab/q3d/test_3.sce +++ /dev/null @@ -1,53 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_utils.sci'); -exec('q3d_fdm.sci'); -exec('q3d_ctl.sci'); -exec('q3d_string.sci'); - -[traj] = string_get_traj(); -dt = 1/512; -[nr,nc]=size(traj); -duration = nc*dt; - - -fdm_init(0,duration); -ctl_init(); - -global ctl_motor; -ctl_motor(:,1) = fdm_mass * fdm_g * [0.5;0.5]; - -for i=1:length(fdm_time)-1 - - fdm_run(i+1, ctl_motor(:,i)); -// sp_pos= [ 0; 0]; - sp_pos = traj(:,i); - ctl_run(i+1, sp_pos); - -end - -//set("current_figure",0); -clf(); -//f=get("current_figure"); -//f.figure_name="CTL"; - -if 1 - clf(); - plot2d(traj(1,:), traj(2,:),1); - plot2d(fdm_state(FDM_SX,:), fdm_state(FDM_SZ,:),2); - plot2d(ctl_ref_0(AXIS_X,:), ctl_ref_0(AXIS_Z,:),3); - pause -end - -if 1 - clf(); - drawlater(); - ctl_display(); - drawnow(); - pause -end -if 1 -gen_video(); -end diff --git a/sw/simulator/scilab/q3d/test_4.sce b/sw/simulator/scilab/q3d/test_4.sce deleted file mode 100644 index 5ab218fa2c..0000000000 --- a/sw/simulator/scilab/q3d/test_4.sce +++ /dev/null @@ -1,11 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_string.sci'); - -[traj1, traj5] = string_get_traj2(); -dt = 1/512; -clf(); -plot2d(traj1(1,:), traj1(2,:), -1,rect=[-2 -0.5 2 3.5]); -plot2d(traj5(1,:), traj5(2,:), 2,rect=[-2 -0.5 2 3.5]); diff --git a/sw/simulator/scilab/q3d/test_5.sce b/sw/simulator/scilab/q3d/test_5.sce deleted file mode 100644 index ca244532fe..0000000000 --- a/sw/simulator/scilab/q3d/test_5.sce +++ /dev/null @@ -1,57 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_utils.sci'); -exec('q3d_fdm.sci'); -exec('q3d_ctl.sci'); -exec('q3d_ref_misc.sci'); - -[time, Xref] = get_reference_circle(); - -clf(); - - -fdm_init(0,time($)); - -ctl_init(); - -global ctl_motor; -ctl_motor(:,1) = fdm_mass * fdm_g * [0.5;0.5]; - -global ctl_ref_0; -global ctl_ref_1; -global ctl_ref_2; -global ctl_ref_3; -global ctl_ref_4; -ctl_ref_0 = Xref(1:2,:); -ctl_ref_1 = Xref(3:4,:); -ctl_ref_2 = Xref(5:6,:); -ctl_ref_3 = Xref(7:8,:); -ctl_ref_4 = Xref(9:10,:); - -global fdm_state; -global ctl_ref_thetad; -fdm_state(FDM_SXD, 1) = ctl_ref_1(AXIS_X,1); -fdm_state(FDM_SZD, 1) = ctl_ref_1(AXIS_Z,1); -ctl_run_flatness(1); -fdm_state(FDM_STHETAD, 1) = ctl_ref_thetad(1); - -for i=1:length(fdm_time)-1 - - fdm_run(i+1, ctl_motor(:,i)); - ctl_run_flatness(i+1); - -end -if 0 -clf(); -plot2d(fdm_state(FDM_SX,:), fdm_state(FDM_SZ,:),2); -plot2d(ctl_ref_0(AXIS_X,:), ctl_ref_0(AXIS_Z,:),3); - pause -end -clf(); -drawlater(); -ctl_display(); -drawnow(); - -gen_video(); \ No newline at end of file diff --git a/sw/simulator/scilab/q3d/test_6.sce b/sw/simulator/scilab/q3d/test_6.sce deleted file mode 100644 index 335240c9d4..0000000000 --- a/sw/simulator/scilab/q3d/test_6.sce +++ /dev/null @@ -1,83 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_utils.sci'); -exec('q3d_fdm.sci'); -exec('q3d_ctl.sci'); -exec('q3d_ref_misc.sci'); -exec('poly_utils.sci'); - -global fdm_state - -[time_loop, ref_loop] = get_reference_circle(); - -a = [-1 0 0 0 0; - 0 0 0 0 0]; - -[time_foo, ref_foo] = get_reference_poly3(1,a ,a); - - -start_loop = [ref_loop(1,1) ref_loop(3,1) ref_loop(5,1) ref_loop(7,1) ref_loop(9,1) - ref_loop(2,1) ref_loop(4,1) ref_loop(6,1) ref_loop(8,1) ref_loop(10,1)]; - -[time_intro, ref_intro] = get_reference_poly3(0.9, a, start_loop); - -end_loop = [ref_loop(1,$),ref_loop(3,$),ref_loop(5,$),ref_loop(7,$),ref_loop(9,$); - ref_loop(2,$),ref_loop(4,$),ref_loop(6,$),ref_loop(8,$),ref_loop(10,$)]; - -c = [ 1 0 0 0 0; - 0 0 0 0 0]; - -[time_outro, ref_outro] = get_reference_poly3(0.9,end_loop,c); - -[time_bar, ref_bar] = get_reference_poly3(1,c ,c); - - -Xref = [ref_foo ref_intro(:,2:$) ref_loop(:,2:$) ref_outro(:,2:$) ref_bar(:,2:$)]; - - -fdm_init(0,time_foo($)+time_intro($)+time_loop($)+time_outro($)+time_bar($)); -fdm_state(FDM_SX,1) = -1; - -ctl_init(); - -global ctl_motor; -ctl_motor(:,1) = fdm_mass * fdm_g * [0.5;0.5]; - -global ctl_ref_0; -global ctl_ref_1; -global ctl_ref_2; -global ctl_ref_3; -global ctl_ref_4; -ctl_ref_0 = Xref(1:2,:); -ctl_ref_1 = Xref(3:4,:); -ctl_ref_2 = Xref(5:6,:); -ctl_ref_3 = Xref(7:8,:); -ctl_ref_4 = Xref(9:10,:); - -//global fdm_state; -fdm_state(FDM_SXD, 1) = ctl_ref_1(AXIS_X,1); -fdm_state(FDM_SZD, 1) = ctl_ref_1(AXIS_Z,1); -ctl_run_flatness(1); -//global ctl_ref_thetad; -fdm_state(FDM_STHETA, 1) = ctl_ref_theta(1); -fdm_state(FDM_STHETAD, 1) = ctl_ref_thetad(1); - -for i=1:length(fdm_time)-1 - - fdm_run(i+1, ctl_motor(:,i)); - ctl_run_flatness(i+1); - -end -if 1 -clf(); -plot2d(fdm_state(FDM_SX,:), fdm_state(FDM_SZ,:),2); -plot2d(ctl_ref_0(AXIS_X,:), ctl_ref_0(AXIS_Z,:),3); - pause -end -clf(); -ctl_display(); - pause - -gen_video(); \ No newline at end of file diff --git a/sw/simulator/scilab/q3d/test_7.sce b/sw/simulator/scilab/q3d/test_7.sce deleted file mode 100644 index 5af14d69a0..0000000000 --- a/sw/simulator/scilab/q3d/test_7.sce +++ /dev/null @@ -1,82 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_utils.sci'); -exec('q3d_fdm.sci'); -exec('q3d_ctl.sci'); -exec('q3d_ref_misc.sci'); - -a = [ 0; 0 - 0; 0 - 0; 0 - 0; 0 - 0; 0 ]; -b = [ 0 ; 0 - 1.2566371; 0 - 0 ; 1.5791367 - -1.9844017; 0 - 0 ; -2.4936727 ]; -[time, Xref] = get_reference_looping(0, 4, [0;0], 2); - -clf(); - -subplot(5,2,1); -plot2d(time, Xref(1,:)); -plot2d(0, a(1), -3); -plot2d(1, b(1), -3); -xtitle('X(0)'); - -subplot(5,2,2); -plot2d(time, Xref(2,:)); -plot2d(0, a(2), -3); -plot2d(1, b(2), -3); -xtitle('Z(0)'); - -subplot(5,2,3); -plot2d(time, Xref(3,:)); -plot2d(0, a(3), -3); -plot2d(1, b(3), -3); -xtitle('X(1)'); - -subplot(5,2,4); -plot2d(time, Xref(4,:)); -plot2d(0, a(4), -3); -plot2d(1, b(4), -3); -xtitle('Z(1)'); - -subplot(5,2,5); -plot2d(time, Xref(5,:)); -plot2d(0, a(5), -3); -plot2d(1, b(5), -3); -xtitle('X(2)'); - -subplot(5,2,6); -plot2d(time, Xref(6,:)); -plot2d(0, a(6), -3); -plot2d(1, b(6), -3); -xtitle('Z(2)'); - -subplot(5,2,7); -plot2d(time, Xref(7,:)); -plot2d(0, a(7), -3); -plot2d(1, b(7), -3); -xtitle('X(3)'); - -subplot(5,2,8); -plot2d(time, Xref(8,:)); -plot2d(0, a(8), -3); -plot2d(1, b(8), -3); -xtitle('Z(3)'); - -subplot(5,2,9); -plot2d(time, Xref(9,:)); -plot2d(0, a(9), -3); -plot2d(1, b(9), -3); -xtitle('X(4)'); - -subplot(5,2,10); -plot2d(time, Xref(10,:)); -plot2d(0, a(10), -3); -plot2d(1, b(10), -3); -xtitle('Z(4)'); diff --git a/sw/simulator/scilab/q3d/test_8.sce b/sw/simulator/scilab/q3d/test_8.sce deleted file mode 100644 index 6cfa7fb245..0000000000 --- a/sw/simulator/scilab/q3d/test_8.sce +++ /dev/null @@ -1,65 +0,0 @@ -clear(); -clearglobal(); - - -exec('q3d_utils.sci'); -exec('q3d_fdm.sci'); -exec('q3d_ctl.sci'); -exec('q3d_ref_misc.sci'); - - - -l_t0 = 0; -l_duration = 2.2; -l_center = [0;0]; -l_radius = 1; -[time, Xref] = get_reference_looping(l_t0, l_duration, l_center, l_radius); - - -fdm_init(0,time($)); - -ctl_init(); - -global ctl_motor; -ctl_motor(:,1) = fdm_mass * fdm_g * [0.5;0.5]; - -global ctl_ref_0; -global ctl_ref_1; -global ctl_ref_2; -global ctl_ref_3; -global ctl_ref_4; -ctl_ref_0 = Xref(1:2,:); -ctl_ref_1 = Xref(3:4,:); -ctl_ref_2 = Xref(5:6,:); -ctl_ref_3 = Xref(7:8,:); -ctl_ref_4 = Xref(9:10,:); - -//global fdm_state; -fdm_state(FDM_SX, 1) = ctl_ref_0(AXIS_X,1); -fdm_state(FDM_SZ, 1) = ctl_ref_0(AXIS_Z,1); -fdm_state(FDM_SXD, 1) = ctl_ref_1(AXIS_X,1); -fdm_state(FDM_SZD, 1) = ctl_ref_1(AXIS_Z,1); -ctl_run_flatness(1); -//global ctl_ref_thetad; -fdm_state(FDM_STHETA, 1) = ctl_ref_theta(1); -fdm_state(FDM_STHETAD, 1) = ctl_ref_thetad(1); - -for i=1:length(fdm_time)-1 - - fdm_run(i+1, ctl_motor(:,i)); - ctl_run_flatness(i+1); - -end -if 1 -clf(); -plot2d(fdm_state(FDM_SX,:), fdm_state(FDM_SZ,:),2); -plot2d(ctl_ref_0(AXIS_X,:), ctl_ref_0(AXIS_Z,:),3); - pause -end -clf(); -drawlater(); -ctl_display(); -drawnow(); - pause - -gen_video(); \ No newline at end of file