Commit 24bc4ac6 authored by mmroma's avatar mmroma
Browse files

Payload tracking is working well now :)

parent 3f93f153
...@@ -2,11 +2,11 @@ function [coarse_WPs] = WPs_set2_square() ...@@ -2,11 +2,11 @@ function [coarse_WPs] = WPs_set2_square()
%WPs_set2 %WPs_set2
h = 0; h = 0;
coarse_WPs = [0, 0, h, 0; coarse_WPs = [0, 0, h, 0;
0, 0, h, 3; 0, 0, h, 4;
1, 0, h, 6; 1, 0, h, 8;
1, 1, h, 9; 1, 1, h, 12;
0, 1, h, 12; 0, 1, h, 16;
0, 0, h, 15; 0, 0, h, 20;
0, 0, h, 18]; 0, 0, h, 24];
end end
...@@ -61,16 +61,8 @@ function [params] = setUpParameters(traj_num) ...@@ -61,16 +61,8 @@ function [params] = setUpParameters(traj_num)
r = params.payload.diameter / 2; r = params.payload.diameter / 2;
%% Adjust Payload Controller Parameters %% Adjust Payload Controller Parameters
int_limit = 1; % meters params.e_load_pos_Kp = [diag([2, 2, 2]), diag([0.7, 0.7, 0.7])];
% params.e_load_pos_Kp = diag([1,1,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_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 %% Load Multirotors
params.num_vehicles = 5; params.num_vehicles = 5;
......
...@@ -28,8 +28,8 @@ plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position ...@@ -28,8 +28,8 @@ plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position
plot_velocity(simData.tout, simData.payload_X, simData.Xd_load,'Payload Velocity','northeast'); plot_velocity(simData.tout, simData.payload_X, simData.Xd_load,'Payload Velocity','northeast');
% plotHapticForceEstimate(simData,'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(:,1:3),'Load Control - Added Position','southwest');
plot_vec3(simData.tout, simData.Xd_load_control(:,4:6),'Load Control - Added Velocity','south'); % 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_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_velocity(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Vel','northeast');
......
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