[paparazzi-commits] [4563]

[antoine drouin]

Revision: 4563
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4563
Author:   poine
Date:     2010-02-23 17:44:54 +0000 (Tue, 23 Feb 2010)
Log Message:
-----------


Added Paths:
-----------
    paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce                 
        (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-23 
17:44:54 UTC (rev 4563)
@@ -0,0 +1,36 @@
+clear();
+
+exec('q6d_sbb.sci');
+exec('q6d_diff_flatness.sci');
+exec('q6d_algebra.sci');
+exec('q6d_display.sci');
+
+t0 = 0;
+t1 = 4.;
+dt = 1/512;
+time = t0:dt:t1;
+
+dyn = [rad_of_deg(500) 0.7; rad_of_deg(500) 0.7];
+max_speed = [5 2.5];
+max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
+
+b0 = [ 0    0   0];
+b1 = [-10   1  -2];
+[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
+
+
+set("current_figure",0);
+clf();
+display_fo_traj(time, fo_traj);
+
+
+diff_flat_cmd = zeros(4,length(time));
+diff_flat_ref = zeros(DF_REF_SIZE, length(time));
+for i=1:length(time)
+//  diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i));
+  diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
+end
+
+set("current_figure",2);
+clf();
+display_df_ref(time, diff_flat_ref);