mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-01 04:46:51 +08:00
This commit is contained in:
Antoine Drouin
@@ -1,274 +1,23 @@
|
||||
|
||||
// X Z Theta Xd Zd Theta_d
|
||||
//ctl_gain = [ 0 -1 0 0 -1 0
|
||||
// -2 0 5 -2 0 5 ];
|
||||
|
||||
|
||||
global ctl_sp_pos;
|
||||
global ctl_ref_0;
|
||||
global ctl_ref_1;
|
||||
global ctl_ref_2;
|
||||
global ctl_ref_3;
|
||||
global ctl_ref_4;
|
||||
|
||||
global ctl_ref_theta;
|
||||
global ctl_ref_thetad;
|
||||
|
||||
CMD_SF = 1;
|
||||
CMD_DF = 2;
|
||||
|
||||
global ctl_cmd;
|
||||
global ctl_motor;
|
||||
|
||||
|
||||
|
||||
|
||||
function ctl_init()
|
||||
|
||||
global fdm_time;
|
||||
|
||||
global ctl_sp_pos;
|
||||
ctl_sp_pos = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_0;
|
||||
ctl_ref_0 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_1;
|
||||
ctl_ref_1 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_2;
|
||||
ctl_ref_2 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_3;
|
||||
ctl_ref_3 = zeros(AXIS_NB, length(fdm_time));
|
||||
global ctl_ref_4;
|
||||
ctl_ref_4 = zeros(AXIS_NB, length(fdm_time));
|
||||
|
||||
global ctl_ref_theta;
|
||||
ctl_ref_theta = zeros(1, length(fdm_time));
|
||||
global ctl_ref_thetad;
|
||||
ctl_ref_thetad = zeros(1, length(fdm_time));
|
||||
|
||||
|
||||
global ctl_cmd;
|
||||
ctl_cmd = zeros(FDM_MOTOR_NB, length(fdm_time));
|
||||
global ctl_motor;
|
||||
ctl_motor = zeros(FDM_MOTOR_NB, length(fdm_time));
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
ctl_mass = 0.25;
|
||||
ctl_inertia = 0.0078;
|
||||
|
||||
function ctl_run(i, sp)
|
||||
|
||||
global ctl_sp_pos;
|
||||
ctl_sp_pos(:,i) = sp;
|
||||
|
||||
ctl_update_ref_4th_order(i);
|
||||
|
||||
ctl_run_flatness(i);
|
||||
if 0
|
||||
ctl_gain = [ 0 0 0 0 0 0
|
||||
0 0 0 0 0 0 ];
|
||||
else
|
||||
ctl_gain = [ 0 -1 0 0 -1 0
|
||||
-0.95 0 3 -1.2 0 3 ];
|
||||
|
||||
endfunction
|
||||
end
|
||||
function [fb_cmd] = ctl_compute_feeback(fdm_state, ref)
|
||||
|
||||
|
||||
|
||||
function ctl_run_flatness(i)
|
||||
|
||||
|
||||
global ctl_ref_theta;
|
||||
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)) / ...
|
||||
((9.81 + ctl_ref_2(AXIS_Z,i))^2+ctl_ref_2(AXIS_X,i)^2);
|
||||
|
||||
global ctl_cmd;
|
||||
global ctl_ref_2;
|
||||
ctl_cmd(CMD_SF,i) = ctl_mass * sqrt(ctl_ref_2(AXIS_X,i)^2 +...
|
||||
(ctl_ref_2(AXIS_Z,i)+9.81)^2);
|
||||
|
||||
x2 = ctl_ref_2(AXIS_X,i);
|
||||
z2p1 = ctl_ref_2(AXIS_Z,i)+9.81;
|
||||
x3 = ctl_ref_3(AXIS_X,i);
|
||||
z3 = ctl_ref_3(AXIS_Z,i);
|
||||
x4 = ctl_ref_4(AXIS_X,i);
|
||||
z4 = ctl_ref_4(AXIS_Z,i);
|
||||
a = x4*z2p1 - z4*x2;
|
||||
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);
|
||||
|
||||
global ctl_motor;
|
||||
A2M = 0.5 * [ 1 1
|
||||
1 -1 ];
|
||||
ctl_motor(:,i) = A2M * ctl_cmd(:,i);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [state] = ctl_state_of_flat_out(ref)
|
||||
|
||||
state = zeros(FDM_SSIZE, 1);
|
||||
state(FDM_SX) = ref(1);
|
||||
state(FDM_SZ) = ref(2);
|
||||
state(FDM_SXD) = ref(3);
|
||||
state(FDM_SZD) = ref(4);
|
||||
theta = -atan(ref(5), 9.81 + ref(6));
|
||||
thetad = -((9.81 + ref(6))*ref(7) - ref(5)*ref(8)) / ...
|
||||
((9.81 + ref(6))^2+ref(5)^2);
|
||||
state(FDM_STHETA) = theta;
|
||||
state(FDM_STHETAD) = thetad;
|
||||
state_err = fdm_state - ref;
|
||||
fb_cmd = ctl_gain * state_err;
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
ctl_omega_ref1 = [ rad_of_deg(90); rad_of_deg(90)];
|
||||
ctl_zeta_ref1 = [ 0.9; 0.9 ];
|
||||
|
||||
ctl_omega_ref2 = [ rad_of_deg(1440); rad_of_deg(1440)];
|
||||
ctl_zeta_ref2 = [ 0.9; 0.9 ];
|
||||
|
||||
a0 = ctl_omega_ref1^2 .* ctl_omega_ref2^2;
|
||||
a1 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_omega_ref2^2 + ...
|
||||
2 * ctl_zeta_ref2 .* ctl_omega_ref2 .* ctl_omega_ref1^2;
|
||||
a2 = ctl_omega_ref1^2 + ...
|
||||
2 * ctl_zeta_ref1 .* ctl_omega_ref1 .* ctl_zeta_ref2 .* ctl_omega_ref2 + ...
|
||||
ctl_omega_ref2^2;
|
||||
a3 = 2 * ctl_zeta_ref1 .* ctl_omega_ref1 + 2 * ctl_zeta_ref2 .* ctl_omega_ref2;
|
||||
//a4 = [1;1];
|
||||
|
||||
function ctl_update_ref_4th_order(i)
|
||||
global ctl_sp_pos;
|
||||
global ctl_ref_0;
|
||||
global ctl_ref_1;
|
||||
global ctl_ref_2;
|
||||
global ctl_ref_3;
|
||||
global ctl_ref_4;
|
||||
|
||||
dt = 1/512;
|
||||
ctl_ref_3(:,i) = ctl_ref_3(:,i-1) + dt * ctl_ref_4(:,i-1);
|
||||
ctl_ref_2(:,i) = ctl_ref_2(:,i-1) + dt * ctl_ref_3(:,i-1);
|
||||
ctl_ref_1(:,i) = ctl_ref_1(:,i-1) + dt * ctl_ref_2(:,i-1);
|
||||
ctl_ref_0(:,i) = ctl_ref_0(:,i-1) + dt * ctl_ref_1(:,i-1);
|
||||
|
||||
err_pos = ctl_ref_0(:,i) - ctl_sp_pos(:,i);
|
||||
ctl_ref_4(:,i) = -a3 .* ctl_ref_3(:,i) -a2 .* ctl_ref_2(:,i) -a1 .* ctl_ref_1(:,i) -a0.*err_pos;
|
||||
|
||||
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function ctl_update_ref_1st_order()
|
||||
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
function ctl_display()
|
||||
nr = 5;
|
||||
nc = 3;
|
||||
subplot(nr,nc,1);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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");
|
||||
xtitle('Theta(1)');
|
||||
|
||||
subplot(nr,nc,7);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_2(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(2)');
|
||||
|
||||
subplot(nr,nc,8);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_2(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(2)');
|
||||
|
||||
subplot(nr,nc,10);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_3(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(3)');
|
||||
|
||||
subplot(nr,nc,11);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_3(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(3)');
|
||||
|
||||
subplot(nr,nc,13);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_4(AXIS_X,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('X(4)');
|
||||
|
||||
subplot(nr,nc,14);
|
||||
// plot2d(fdm_time, ins_state(INS_SX,:),2);
|
||||
plot2d(fdm_time, ctl_ref_4(AXIS_Z,:),3);
|
||||
legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('Z(4)');
|
||||
|
||||
subplot(nr,nc,9);
|
||||
plot2d(fdm_time, ctl_cmd(CMD_SF,:),3);
|
||||
// legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('U_sigma');
|
||||
|
||||
subplot(nr,nc,12);
|
||||
plot2d(fdm_time, ctl_cmd(CMD_DF,:),3);
|
||||
// legends(["setpoint", "INS", "ref"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('U_delta');
|
||||
|
||||
subplot(nr,nc,15);
|
||||
plot2d(fdm_time, ctl_motor(FDM_MOTOR_RIGHT,:),2);
|
||||
plot2d(fdm_time, ctl_motor(FDM_MOTOR_LEFT,:),3);
|
||||
plot2d(fdm_time, fdm_min_thrust * ones(1,length(fdm_time)),5);
|
||||
plot2d(fdm_time, fdm_max_thrust * ones(1,length(fdm_time)),5);
|
||||
// legends(["setpoint", "RIGHT", "LEFT"],[5 2 3], with_box=%f, opt="ur");
|
||||
xtitle('U_motors');
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
@@ -10,7 +10,9 @@ DF_ORANK = 5; // Number of time derivative needed
|
||||
|
||||
fo_g = 9.81;
|
||||
fo_mass = 0.25;
|
||||
fo_inertia = 0.0078;
|
||||
fo_J = 0.0078;
|
||||
fo_Ct0 = 4. * fo_mass * fo_g / 2;
|
||||
fo_la = 0.25;
|
||||
|
||||
global fo_traj;
|
||||
|
||||
@@ -32,19 +34,19 @@ endfunction
|
||||
function [inp] = df_input_of_fo(fo)
|
||||
|
||||
x2 = fo(1,3);
|
||||
z2p1 = fo(2,3)+9.81;
|
||||
z2pg = fo(2,3)+9.81;
|
||||
|
||||
u1 = fo_mass * sqrt((x2)^2 + (z2p1)^2);
|
||||
u1 = fo_mass / fo_Ct0 * sqrt((x2)^2 + (z2pg)^2);
|
||||
|
||||
x3 = fo(1,4);
|
||||
z3 = fo(2,4);
|
||||
x4 = fo(1,5);
|
||||
z4 = fo(2,5);
|
||||
a = x4*z2p1 - z4*x2;
|
||||
b = z2p1^2+x2^2;
|
||||
c = 2 * (z2p1*z3 + x2*x3);
|
||||
d = x3*z2p1-z3*x2;
|
||||
u2 = -fo_inertia * ( a/b - c*d/b^2);
|
||||
a = x4*z2pg - z4*x2;
|
||||
b = z2pg^2+x2^2;
|
||||
c = 2 * (z2pg*z3 + x2*x3);
|
||||
d = x3*z2pg-z3*x2;
|
||||
u2 = fo_J / fo_la /fo_Ct0 * ( a/b - c*d/b^2);
|
||||
|
||||
inp = [u1; u2];
|
||||
|
||||
|
||||
@@ -127,4 +127,72 @@ function display_fo_ref(time, diff_flat_ref)
|
||||
plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :)));
|
||||
xtitle('Thetad');
|
||||
|
||||
endfunction
|
||||
endfunction
|
||||
|
||||
function display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd );
|
||||
|
||||
f=get("current_figure");
|
||||
f.figure_name="Control";
|
||||
|
||||
clf();
|
||||
|
||||
subplot(4,3,1);
|
||||
plot2d(time, diff_flat_ref(FDM_SX, :), 3);
|
||||
plot2d(time, fdm_state(FDM_SX, :), 2);
|
||||
legends(["fdm", "ref"],[2 3], with_box=%f, opt="ul");
|
||||
xtitle('X');
|
||||
|
||||
subplot(4,3,2);
|
||||
plot2d(time, diff_flat_ref(FDM_SZ, :), 3);
|
||||
plot2d(time, fdm_state(FDM_SZ, :), 2);
|
||||
xtitle('Z');
|
||||
|
||||
subplot(4,3,3);
|
||||
plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETA, :)), 3);
|
||||
plot2d(time, deg_of_rad(fdm_state(FDM_STHETA, :)), 2);
|
||||
xtitle('Theta');
|
||||
|
||||
subplot(4,3,4);
|
||||
plot2d(time, diff_flat_ref(FDM_SXD, :), 3);
|
||||
plot2d(time, fdm_state(FDM_SXD, :), 2);
|
||||
xtitle('Xd');
|
||||
|
||||
subplot(4,3,5);
|
||||
plot2d(time, diff_flat_ref(FDM_SZD, :), 3);
|
||||
plot2d(time, fdm_state(FDM_SZD, :), 2);
|
||||
xtitle('Zd');
|
||||
|
||||
subplot(4,3,6);
|
||||
plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :)), 3);
|
||||
plot2d(time, deg_of_rad(fdm_state(FDM_STHETAD, :)), 2);
|
||||
xtitle('Thetad');
|
||||
|
||||
|
||||
subplot(4,2,5);
|
||||
// plot2d(time, diff_flat_cmd(1,:)+ fb_cmd(1,:), 5);
|
||||
xset("color",5);
|
||||
foo = diff_flat_cmd(1,:) + fb_cmd(1,:);
|
||||
xfpoly([time time($:-1:1)], [diff_flat_cmd(1,:) foo($:-1:1)]);
|
||||
xset("color",1);
|
||||
plot2d(time, diff_flat_cmd(1,:), 2);
|
||||
xtitle('u_t');
|
||||
|
||||
subplot(4,2,6);
|
||||
tot_cmd = diff_flat_cmd(2,:) + fb_cmd(2,:);
|
||||
xset("color",5);
|
||||
xfpoly([time time($:-1:1)], [diff_flat_cmd(2,:) tot_cmd($:-1:1)]);
|
||||
xset("color",1);
|
||||
plot2d(time, diff_flat_cmd(2,:), 2);
|
||||
xtitle('u_d');
|
||||
|
||||
subplot(4,2,7);
|
||||
plot2d(time, motor_cmd(1,:), 2);
|
||||
xtitle('u1');
|
||||
|
||||
subplot(4,2,8);
|
||||
plot2d(time, motor_cmd(2,:), 2);
|
||||
xtitle('u2');
|
||||
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
@@ -31,11 +31,16 @@ FDM_MOTOR_NB = 2;
|
||||
fdm_g = 9.81;
|
||||
fdm_mass = 0.25;
|
||||
fdm_inertia = 0.0078;
|
||||
fdm_la = 0.25; // arm length
|
||||
|
||||
fdm_min_thrust = 0.5 * 0.1 * fdm_mass * fdm_g; // 10% of hovering power for each motor
|
||||
fdm_max_thrust = 0.5 * 4.0 * fdm_mass * fdm_g; // 400% of hovering power for each motor
|
||||
fdm_Ct0 = 4. * fdm_mass * fdm_g / 2; // thrust coefficient
|
||||
fdm_V0 = 1e9; //
|
||||
fdm_Cd = 1e-2; // drag coefficient
|
||||
|
||||
fdm_dt = 1./512.;
|
||||
fdm_min_thrust = 0.05; // 5%
|
||||
fdm_max_thrust = 1.0; // 400%
|
||||
|
||||
fdm_wind = [0 0]';
|
||||
|
||||
global fdm_time;
|
||||
global fdm_state;
|
||||
@@ -69,12 +74,29 @@ endfunction
|
||||
function [Xdot] = fdm_get_derivatives(t, X, U)
|
||||
|
||||
Xdot = zeros(length(X),1);
|
||||
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_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));
|
||||
|
||||
Xdot(FDM_SX:FDM_STHETA) = X(FDM_SXD:FDM_STHETAD);
|
||||
|
||||
// forces :
|
||||
gspeed_ltp = X(FDM_SXD:FDM_SZD);
|
||||
airspeed_ltp = gspeed_ltp - fdm_wind;
|
||||
|
||||
stheta = sin(X(FDM_STHETA));
|
||||
ctheta = cos(X(FDM_STHETA));
|
||||
|
||||
DCM = [ctheta stheta ; -stheta ctheta];
|
||||
airspeed_body = DCM * airspeed_ltp;
|
||||
|
||||
lift_body = [0; sum(U) * fdm_Ct0 * ( 1 - abs(1/fdm_V0 * airspeed_body(AXIS_Z)))];
|
||||
lift_ltp = DCM'*lift_body;
|
||||
weight_ltp = [0; -fdm_g * fdm_mass];
|
||||
drag_ltp = -fdm_Cd * norm(airspeed_ltp) * airspeed_ltp;
|
||||
Xdot(FDM_SXD:FDM_SZD) = 1/fdm_mass*(lift_ltp+weight_ltp+drag_ltp);
|
||||
|
||||
// moments
|
||||
Xdot(FDM_STHETAD) = fdm_la * fdm_Ct0 / fdm_inertia*(U(FDM_MOTOR_RIGHT) - U(FDM_MOTOR_LEFT));
|
||||
|
||||
// pause
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
@@ -6,13 +6,14 @@ exec('q3d_sbb.sci');
|
||||
exec('q3d_fo_traj_misc.sci');
|
||||
|
||||
exec('q3d_diff_flatness.sci');
|
||||
exec('q3d_ctl.sci');
|
||||
exec('q3d_fdm.sci');
|
||||
exec('q3d_display.sci');
|
||||
exec('q3d_povray.sci');
|
||||
|
||||
|
||||
t0 = 0;
|
||||
t1 = 10.;
|
||||
t1 = 3.;
|
||||
dt = 1/512;
|
||||
time = t0:dt:t1;
|
||||
|
||||
@@ -23,18 +24,19 @@ if (0)
|
||||
[coefs] = poly_get_coef_from_bound(time, b0, b1);
|
||||
[fo_traj] = poly_gen_traj(time, coefs);
|
||||
end
|
||||
if (0)
|
||||
// differential equation
|
||||
if (1)
|
||||
// differential equation
|
||||
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(29.983325)) 0.5*9.81];
|
||||
b0 = [-5 0];
|
||||
b1 = [ 5 -2];
|
||||
b1 = [ 5 -5];
|
||||
[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
|
||||
printf('xfinal %f, zfinal:%f\n',fo_traj(1,1,$), fo_traj(2,1,$));
|
||||
end
|
||||
if (1)
|
||||
[fo_traj] = fo_traj_circle(time, [0 0], 2, rad_of_deg(45));
|
||||
if (0)
|
||||
// analytic definition
|
||||
[fo_traj] = fo_traj_circle(time, [0 0], 2, rad_of_deg(45));
|
||||
end
|
||||
|
||||
|
||||
@@ -45,13 +47,14 @@ for i=1:length(time)
|
||||
diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
|
||||
end
|
||||
|
||||
fdm_init(time, df_state_of_fo(fo_traj(:,:,1)));
|
||||
fdm_init(time, df_state_of_fo(fo_traj(:,:,1)));
|
||||
fb_cmd = zeros(2,length(time));
|
||||
motor_cmd = zeros(2,length(time));
|
||||
for i=2:length(time)
|
||||
u1 = diff_flat_cmd(1,i-1);
|
||||
u2 = diff_flat_cmd(2,i-1);
|
||||
m1 = 0.5*(u1+u2);
|
||||
m2 = 0.5*(u1-u2);
|
||||
fdm_run(i, [m1 m2]')
|
||||
fb_cmd(:,i-1) = ctl_compute_feeback(fdm_state(:,i-1),diff_flat_ref(:,i-1));
|
||||
MotorsOfCmds = 0.5*[1 -1 ; 1 1];
|
||||
motor_cmd(:,i-1) = MotorsOfCmds * ( diff_flat_cmd(:,i-1) + fb_cmd(:,i-1));
|
||||
fdm_run(i, motor_cmd(:,i-1));
|
||||
end
|
||||
|
||||
set("current_figure",0);
|
||||
@@ -60,23 +63,27 @@ f=get("current_figure");
|
||||
f.figure_name="Flat Outputs Trajectory";
|
||||
display_fo(time, fo_traj);
|
||||
|
||||
if 0
|
||||
set("current_figure",1);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="Commands";
|
||||
display_commands(time, diff_flat_cmd);
|
||||
|
||||
set("current_figure",2);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="Reference";
|
||||
display_fo_ref(time, diff_flat_ref);
|
||||
|
||||
set("current_figure",3);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="FDM";
|
||||
display_fdm();
|
||||
end
|
||||
|
||||
set("current_figure",1);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="Commands";
|
||||
display_commands(time, diff_flat_cmd);
|
||||
display_control(time, diff_flat_ref, fdm_state, diff_flat_cmd, fb_cmd, motor_cmd );
|
||||
|
||||
set("current_figure",2);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="Reference";
|
||||
display_fo_ref(time, diff_flat_ref);
|
||||
|
||||
set("current_figure",3);
|
||||
clf();
|
||||
f=get("current_figure");
|
||||
f.figure_name="FDM";
|
||||
display_fdm();
|
||||
|
||||
|
||||
povray_draw(time, diff_flat_ref);
|
||||
//povray_draw(time, diff_flat_ref);
|
||||
|
||||
Reference in New Issue
Block a user