sw/simulator/scilab/q3d/poly_utils.sci

@@ -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
-
-
-

sw/simulator/scilab/q3d/test_12.sce

@@ -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();

sw/simulator/scilab/q3d/test_2.sce

@@ -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

sw/simulator/scilab/q3d/test_3.sce

@@ -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

sw/simulator/scilab/q3d/test_4.sce

@@ -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]);

sw/simulator/scilab/q3d/test_5.sce

@@ -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();

sw/simulator/scilab/q3d/test_6.sce

@@ -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();

sw/simulator/scilab/q3d/test_7.sce

@@ -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)');

sw/simulator/scilab/q3d/test_8.sce

@@ -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();