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()
%WPs_set2
h = 0;
coarse_WPs = [0, 0, h, 0;
0, 0, h, 3;
1, 0, h, 6;
1, 1, h, 9;
0, 1, h, 12;
0, 0, h, 15;
0, 0, h, 18];
0, 0, h, 4;
1, 0, h, 8;
1, 1, h, 12;
0, 1, h, 16;
0, 0, h, 20;
0, 0, h, 24];
end
......@@ -61,16 +61,8 @@ function [params] = setUpParameters(traj_num)
r = params.payload.diameter / 2;
%% 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_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);
params.e_load_pos_Kp = [diag([2, 2, 2]), diag([0.7, 0.7, 0.7])];
%% Load Multirotors
params.num_vehicles = 5;
......
......@@ -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');
% 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_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');
......
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