Commit 3f93f153 authored by mmroma's avatar mmroma
Browse files

Compute quad guidance in real time now. Also adding velocity gain to payload controller.

parent ea8401d6
......@@ -86,7 +86,9 @@ function [params] = compute_quad_guidance_2(params)
rpy = zeros(6,size(r_dot_d_Vi_G,2));
rpy(1:2,:) = [phi_i{i}; theta_i{i}] * ones(1,size(r_dot_d_Vi_G,2));
quads_xd{i} = [r_d_Vi_G; r_dot_d_Vi_G; rpy];
% quads_xd{i} = [r_d_Vi_G; r_dot_d_Vi_G; rpy];
quads_xd{i} = [r_d_Vi_G; zeros(3,size(params.payload.xd,2)); rpy];
end
......
......@@ -63,12 +63,14 @@ function [params] = setUpParameters(traj_num)
%% Adjust Payload Controller Parameters
int_limit = 1; % meters
% params.e_load_pos_Kp = diag([1,1,1]);
params.e_load_pos_Kp = diag([0.2,0.2,1]);
params.e_load_pos_Ki = diag([0.01,0.01,0.1]);
% params.e_load_pos_Kp = diag([0.2,0.2,1]);
params.e_load_pos_Kp = diag([0.0,0.0,0.0, 0.0,0.0,0.0]);
% params.e_load_pos_Ki = diag([0.01,0.01,0.1]);
params.e_load_pos_Ki = diag([0.0,0.0,0.1, 0.0,0.0,0.0]);
% params.e_load_pos_Kd = diag([0,0,0]);;
params.e_load_pos_int_LL = [-int_limit;-int_limit;-int_limit];
params.e_load_pos_int_UL = [int_limit;int_limit;int_limit];
params.e_load_pos_int = zeros(3,1);
params.e_load_int_LL = -int_limit.*ones(6,1);
params.e_load_int_UL = int_limit.*ones(6,1);
% params.e_load_pos_int = zeros(3,1);
%% Load Multirotors
params.num_vehicles = 5;
......
function [] = plot_orientation(T,X,Xd, simData)
function [] = plot_orientation(T,X,Xd,title_string,location)
figure();
subplot(3,1,1);
plot(T,X(:,7));
hold on;
plot(T,simData.q1_phi_des,'r--');
plot(T,Xd(:,7),'r--');
hold off;
ylabel('\phi (rad)');
xlabel('Time (s)');
title('Attitude');
title(title_string);
legend('Actual','Desired');
% ylim([-pi/2 pi/2]);
......@@ -27,7 +27,7 @@ function [] = plot_orientation(T,X,Xd, simData)
subplot(3,1,2);
plot(T,X(:,8));
hold on;
plot(T,simData.q1_theta_des,'r--');
plot(T,Xd(:,8),'r--');
hold off;
ylabel('\theta (rad)');
xlabel('Time (s)');
......@@ -43,6 +43,8 @@ function [] = plot_orientation(T,X,Xd, simData)
xlabel('Time (s)');
% ylim([-pi/2 pi/2]);
legend('Actual','Desired');
movegui(location);
end
function [] = plot_velocity(T,X,Xd, simData)
function [] = plot_velocity(T,X,Xd,title_string,location)
figure();
subplot(3,1,1);
plot(T,X(:,4));
hold on;
plot(T,simData.q1_x_dot_des,'r--');
plot(T,Xd(:,4),'r--');
hold off;
ylabel('x_{dot} (m/s)');
title('Velocity');
title(title_string);
legend('Actual','Desired');
subplot(3,1,2);
plot(T,X(:,5));
hold on;
plot(T,simData.q1_y_dot_des,'r--');
plot(T,Xd(:,5),'r--');
hold off;
ylabel('y_{dot} (m/s)');
legend('Actual','Desired');
......@@ -22,11 +22,12 @@ function [] = plot_velocity(T,X,Xd, simData)
subplot(3,1,3);
plot(T,X(:,6));
hold on;
plot(T,simData.q1_z_dot_des,'r--');
plot(T,Xd(:,6),'r--');
hold off;
ylabel('z_{dot} (m/s)');
xlabel('Time (s)');
legend('Actual','Desired');
movegui(location);
end
......@@ -25,12 +25,15 @@ simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFina
%% Plot
plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position','northwest');
plotHapticForceEstimate(simData,'northeast');
plot_velocity(simData.tout, simData.payload_X, simData.Xd_load,'Payload Velocity','northeast');
% plotHapticForceEstimate(simData,'northeast');
plot_vec3(simData.tout, simData.Xd_load_control(:,1:3),'Load Control - Added Position','southwest');
plot_vec3(simData.tout, simData.Xd_load_control(:,4:6),'Load Control - Added Velocity','south');
%
plot_position(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Position','southeast');
plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Vel','northeast');
plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Attitude','north');
% plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment