diff --git a/sw/simulator/scilab/q6d/test_stop_stop.sce b/sw/simulator/scilab/q6d/test_stop_stop.sce new file mode 100644 index 0000000000..405e315293 --- /dev/null +++ b/sw/simulator/scilab/q6d/test_stop_stop.sce @@ -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);