From 3d09e8a185ef7aa4c13c16db2e30bb33cc0c3bdc Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Fri, 13 Mar 2009 01:35:58 +0000 Subject: [PATCH] *** empty log message *** --- sw/simulator/scilab/q1d/q1d_ctl_common.sci | 46 +++++++++--------- sw/simulator/scilab/q1d/q1d_run.sce | 14 +++--- sw/simulator/scilab/q3d/q3d_ctl.sci | 28 ++++++----- sw/simulator/scilab/q3d/q3d_fdm.sci | 2 +- sw/simulator/scilab/q3d/q3d_utils.sci | 54 ++++++++++++++++++++++ sw/simulator/scilab/q3d/test_2.sce | 19 +++++--- 6 files changed, 113 insertions(+), 50 deletions(-) diff --git a/sw/simulator/scilab/q1d/q1d_ctl_common.sci b/sw/simulator/scilab/q1d/q1d_ctl_common.sci index db257da0b8..877ec3e8df 100644 --- a/sw/simulator/scilab/q1d/q1d_ctl_common.sci +++ b/sw/simulator/scilab/q1d/q1d_ctl_common.sci @@ -17,10 +17,12 @@ ctl_min_cmd = 0.01; // actuators low saturation ref_omega = rad_of_deg(200.);// second order linear dynamics ref_zeta = 0.85; ref_saturate = 1; // with saturations -ref_max_accel = 2. * fdm_g; // m/s2 - aka (fdm_max_thrust/fdm_mass - 1) fdm_g -ref_min_accel = -0.9 * fdm_g;// m/s2 -ref_max_speed = 3.; // m/s - +ref_max_accel = 2. * 9.81; // m/s2 - aka (fdm_max_thrust/fdm_mass - 1) fdm_g +ref_min_accel = -0.9 * 9.81; // m/s2 +ref_max_speed = 3.; // m/s +ref_min_speed = -3.; // m/s +ref_thau_z = 0.35; +ref_thau_zd = 0.12; // // propagation of a second order linear model with saturations @@ -29,29 +31,25 @@ function [Xrefi1] = ctl_update_ref(Xrefi, Xspi, dt) Xrefi1 = zeros(FDM_SIZE, 1); - // second order ref model Xrefi1(REF_Z) = Xrefi(REF_Z) + dt * Xrefi(REF_ZD); Xrefi1(REF_ZD) = Xrefi(REF_ZD) + dt * Xrefi(REF_ZDD); - Xrefi1(REF_ZDD) = -2*ref_zeta*ref_omega * Xrefi(REF_ZD) ... - -ref_omega^2 * ( Xrefi(REF_Z) - Xspi); - if ref_saturate - // saturate acceleration - if Xrefi1(REF_ZDD) > ref_max_accel - Xrefi1(REF_ZDD) = ref_max_accel; - elseif Xrefi1(REF_ZDD) < ref_min_accel - Xrefi1(REF_ZDD) = ref_min_accel; - end - // saturate speed - if Xrefi1(REF_ZD) >= ref_max_speed & Xrefi1(REF_ZDD) >= 0 - Xrefi1(REF_ZD) = ref_max_speed; - Xrefi1(REF_ZDD) = 0; - elseif Xrefi1(REF_ZD) <= -ref_max_speed & Xrefi1(REF_ZDD) <= 0 - Xrefi1(REF_ZD) = -ref_max_speed; - Xrefi1(REF_ZDD) = 0; - end - end - + err_z = Xrefi1(REF_Z) - Xspi; + sp_zd = -1/ref_thau_z*err_z; + if sp_zd >= 0 + sp_zd = min(sp_zd, ref_max_speed); + else + sp_zd = max(sp_zd, ref_min_speed); + end + err_zd = Xrefi1(REF_ZD) - sp_zd; + sp_zdd = -1/ref_thau_zd*err_zd; + if sp_zdd >= 0 + sp_zdd = min(sp_zdd, ref_max_accel); + else + sp_zdd = max(sp_zdd, ref_min_accel); + end + Xrefi1(REF_ZDD) = sp_zdd; + endfunction // diff --git a/sw/simulator/scilab/q1d/q1d_run.sce b/sw/simulator/scilab/q1d/q1d_run.sce index c0682e1597..e859b198c8 100644 --- a/sw/simulator/scilab/q1d/q1d_run.sce +++ b/sw/simulator/scilab/q1d/q1d_run.sce @@ -1,12 +1,12 @@ clear(); -exec("tvc_utils.sci"); -exec("tvc_fdm.sci"); -exec("tvc_sensors.sci"); -exec("tvc_ins.sci"); -exec("tvc_ctl_common.sci"); -exec("tvc_ctl_miac.sci"); -exec("tvc_ctl_mrac.sci"); +exec("q1d_utils.sci"); +exec("q1d_fdm.sci"); +exec("q1d_sensors.sci"); +exec("q1d_ins.sci"); +exec("q1d_ctl_common.sci"); +exec("q1d_ctl_miac.sci"); +exec("q1d_ctl_mrac.sci"); // diff --git a/sw/simulator/scilab/q3d/q3d_ctl.sci b/sw/simulator/scilab/q3d/q3d_ctl.sci index 7f1a8370b4..f2fcef6b27 100644 --- a/sw/simulator/scilab/q3d/q3d_ctl.sci +++ b/sw/simulator/scilab/q3d/q3d_ctl.sci @@ -61,7 +61,7 @@ function ctl_run(i) if fdm_time(i) < 4 ctl_sp_pos(:,i)= [ 1; 0]; else - ctl_sp_pos(:,i)= [ 0; 0]; + ctl_sp_pos(:,i)= [ 0; 1]; end ctl_update_ref_4th_order(i); @@ -81,11 +81,11 @@ function ctl_run(i) b = z2p1^2+x2^2; c = 2 * (z2p1*z3 + x2*x3); d = x3*z2p1-z3*x2; - ctl_cmd(CMD_DF,i) = ctl_inertia * ( a/b - c*d/b^2); + ctl_cmd(CMD_DF,i) = -ctl_inertia * ( a/b - c*d/b^2); global ctl_motor; A2M = 0.5 * [ 1 1 - 1 -1 ]; + 1 -1 ]; ctl_motor(:,i) = A2M * ctl_cmd(:,i); endfunction @@ -126,9 +126,9 @@ function ctl_update_ref_4th_order(i) ctl_ref_4(:,i) = -a3 .* ctl_ref_3(:,i) -a2 .* ctl_ref_2(:,i) -a1 .* ctl_ref_1(:,i) -a0.*err_pos; global ctl_ref_theta; - ctl_ref_theta(i) = atan(ctl_ref_2(AXIS_X,i), 9.81 + ctl_ref_2(AXIS_Z,i)); + ctl_ref_theta(i) = -atan(ctl_ref_2(AXIS_X,i), 9.81 + ctl_ref_2(AXIS_Z,i)); global ctl_ref_thetad; - ctl_ref_thetad(i) = ((9.81 + ctl_ref_2(AXIS_Z,i))*ctl_ref_3(AXIS_X,i) - ctl_ref_2(AXIS_X,i)*ctl_ref_3(AXIS_Z,i)) / ... + ctl_ref_thetad(i) = -((9.81 + ctl_ref_2(AXIS_Z,i))*ctl_ref_3(AXIS_X,i) - ctl_ref_2(AXIS_X,i)*ctl_ref_3(AXIS_Z,i)) / ... ((9.81 + ctl_ref_2(AXIS_Z,i))^2+ctl_ref_2(AXIS_X,i)^2); @@ -148,40 +148,46 @@ function ctl_display() nr = 5; nc = 3; subplot(nr,nc,1); - plot2d(fdm_time, fdm_state(FDM_SX,:),2); + plot_with_min_rect(fdm_time, fdm_state(FDM_SX,:),2, -0.5, 0.5); +// plot2d(fdm_time, fdm_state(FDM_SX,:),2); plot2d(fdm_time, ctl_ref_0(AXIS_X,:),3); plot2d(fdm_time, ctl_sp_pos(AXIS_X,:),5); legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); xtitle('X(0)'); subplot(nr,nc,2); - plot2d(fdm_time, fdm_state(FDM_SZ,:),2); + plot_with_min_rect(fdm_time, fdm_state(FDM_SZ,:),2, -0.5, 0.5); +// plot2d(fdm_time, fdm_state(FDM_SZ,:),2); plot2d(fdm_time, ctl_ref_0(AXIS_Z,:),3); plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5); legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); xtitle('Z(0)'); subplot(nr,nc,3); - plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2); + plot_with_min_rect(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2, -1., 1.); +// plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETA,:)),2); plot2d(fdm_time, deg_of_rad(ctl_ref_theta),3); // plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5); legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); xtitle('Theta(0)'); subplot(nr,nc,4); - plot2d(fdm_time, fdm_state(FDM_SXD,:),2); + plot_with_min_rect(fdm_time, fdm_state(FDM_SXD,:),2, -0.5, 0.5); +// plot2d(fdm_time, fdm_state(FDM_SXD,:),2); plot2d(fdm_time, ctl_ref_1(AXIS_X,:),3); legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); xtitle('X(1)'); subplot(nr,nc,5); - plot2d(fdm_time, fdm_state(FDM_SZD,:),2); + plot_with_min_rect(fdm_time, fdm_state(FDM_SZD,:),2, -0.5, 0.5); +// plot2d(fdm_time, fdm_state(FDM_SZD,:),2); plot2d(fdm_time, ctl_ref_1(AXIS_Z,:),3); legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); xtitle('Z(1)'); subplot(nr,nc,6); - plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2); + plot_with_min_rect(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2, -1., 1.); + //plot2d(fdm_time, deg_of_rad(fdm_state(FDM_STHETAD,:)),2); plot2d(fdm_time, deg_of_rad(ctl_ref_thetad),3); // plot2d(fdm_time, ctl_sp_pos(AXIS_Z,:),5); legends(["setpoint", "fdm", "ref"],[5 2 3], with_box=%f, opt="ur"); diff --git a/sw/simulator/scilab/q3d/q3d_fdm.sci b/sw/simulator/scilab/q3d/q3d_fdm.sci index bcfaeb5a06..86b7e87028 100644 --- a/sw/simulator/scilab/q3d/q3d_fdm.sci +++ b/sw/simulator/scilab/q3d/q3d_fdm.sci @@ -58,7 +58,7 @@ function [Xdot] = fdm_get_derivatives(t, X, U) Xdot(FDM_SX) = X(FDM_SXD); Xdot(FDM_SZ) = X(FDM_SZD); Xdot(FDM_STHETA) = X(FDM_STHETAD); - Xdot(FDM_SXD) = sum(U)/fdm_mass*sin(X(FDM_STHETA)); + Xdot(FDM_SXD) = -sum(U)/fdm_mass*sin(X(FDM_STHETA)); Xdot(FDM_SZD) = 1/fdm_mass*(sum(U)*cos(X(FDM_STHETA))-fdm_mass*fdm_g); Xdot(FDM_STHETAD) = 1/fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT)); diff --git a/sw/simulator/scilab/q3d/q3d_utils.sci b/sw/simulator/scilab/q3d/q3d_utils.sci index 86df9d31e6..5781f322f0 100644 --- a/sw/simulator/scilab/q3d/q3d_utils.sci +++ b/sw/simulator/scilab/q3d/q3d_utils.sci @@ -32,3 +32,57 @@ function [_o] = trim_vect(_i,_min, _max) end end endfunction + + +function plot_with_min_rect(t, v, c, min_y, max_y) + max_y = max(max_y, max(v)); + min_y = min(min_y, min(v)); + _rect = [t(1) min_y, t($), max_y]; + plot2d(t, v, c, rect=_rect); +endfunction + + + + +function draw_quad(i) + + global fdm_state; + body_lines = list([-0.5 0; 0.5 0], [-0.8 0.08; -0.2 0.08], [0.2 0.08; 0.8 0.08]); + dcmt = [ cos(fdm_state(FDM_STHETA,i)) -sin(fdm_state(FDM_STHETA,i)) + sin(fdm_state(FDM_STHETA,i)) cos(fdm_state(FDM_STHETA,i)) ]; + _rect = [ -1 -1 1 1]; + earth_lines = list(); + plot2d(fdm_state(FDM_SX,i), fdm_state(FDM_SZ,i),3); + for j=1:length(body_lines) + earth_lines(j) = dcmt*body_lines(j)'; + plot2d(earth_lines(j)(1,:)+fdm_state(FDM_SX,i),earth_lines(j)(2,:)+fdm_state(FDM_SZ,i),1, rect=_rect); + end + +endfunction + + +function gen_video() + + dt_display = 1/25; + + time_display = 0; + for i=1:length(fdm_time) + if fdm_time(i) >= time_display + set("current_figure",0); + f=get("current_figure"); + f.figure_name="CTL"; + clf(); + drawlater(); + draw_quad(i); +// filename = sprintf('images/frame_%03d.gif',i); + filename = sprintf('images/frame_%03d.ppm',i); +// xs2gif(0, filename); + xs2ppm(0, filename); + drawnow(); + pause + time_display = time_display + dt_display; + end + end + +endfunction + diff --git a/sw/simulator/scilab/q3d/test_2.sce b/sw/simulator/scilab/q3d/test_2.sce index c656823b3b..55296f7935 100644 --- a/sw/simulator/scilab/q3d/test_2.sce +++ b/sw/simulator/scilab/q3d/test_2.sce @@ -8,7 +8,7 @@ exec('q3d_ctl.sci'); -fdm_init(0,8.0); +fdm_init(0,7.0); ctl_init(); global ctl_motor; @@ -21,10 +21,15 @@ for i=1:length(fdm_time)-1 end -set("current_figure",0); +//set("current_figure",0); clf(); -f=get("current_figure"); -f.figure_name="CTL"; -drawlater(); -ctl_display(); -drawnow(); \ No newline at end of file +//f=get("current_figure"); +//f.figure_name="CTL"; + +if 1 + drawlater(); + ctl_display(); + drawnow(); +end +pause +gen_video();