Commit 3284d591 authored by mmroma's avatar mmroma
Browse files

Finished transferring all features to simulink. This branch is now ready to be...

Finished transferring all features to simulink. This branch is now ready to be merged back into master :)
parent 3991274a
...@@ -3,9 +3,11 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData) ...@@ -3,9 +3,11 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData)
Ts = params.multirotors{1}.ctlDt; Ts = params.multirotors{1}.ctlDt;
%% 1) Obtain Payload Acceleration from Actual Net Force, Rotation, and Mass %% 1) Obtain Payload Acceleration from Actual Net Force, Rotation, and Mass
F_net_P_P = simData.F_nets(params.num_vehicles*3+1:end,:); % F_net_P_P = simData.F_nets(params.num_vehicles*3+1:end,:);
F_net_P_P = simData.F_net_payload';
m_P = params.payload.m; m_P = params.payload.m;
X_P = simData.X(params.num_vehicles*12+1:end,:); % X_P = simData.X(params.num_vehicles*12+1:end,:);
X_P = simData.payload_X';
num_points = size(X_P,2); num_points = size(X_P,2);
a_P_G = zeros(3,num_points); a_P_G = zeros(3,num_points);
...@@ -19,9 +21,12 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData) ...@@ -19,9 +21,12 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData)
% Tether Forces % Tether Forces
F_Tethers_G = zeros(3,num_points); F_Tethers_G = zeros(3,num_points);
simData.F_tethers_P
for i=1:num_points for i=1:num_points
R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i)); R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
F_Tethers_G(:,i) = R_P_G * simData.F_tethers(params.num_vehicles*3+1:end,i); F_Tethers_G(:,i) = R_P_G * simData.F_tethers_P(i,:)';
end end
...@@ -32,18 +37,27 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData) ...@@ -32,18 +37,27 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData)
F_Drag_P_G = zeros(3,num_points); F_Drag_P_G = zeros(3,num_points);
for i=1:num_points for i=1:num_points
R_P_G = rpy_to_R(X_P(7), X_P(8), X_P(9)); R_P_G = rpy_to_R(X_P(7), X_P(8), X_P(9));
F_Drag_P_G(:,i) = R_P_G * simData.F_drags(params.num_vehicles*3+1:end,i); F_Drag_P_G(:,i) = R_P_G * simData.F_drag_payload(i,:)';
end end
%% 3) Send to variable %% 3) Resample at controller Rate (interpolate)
t = simData.tout(1) : Ts : simData.tout(end);
num_points = length(t);
a_P_G = interp1(simData.tout,a_P_G',t);
F_Drag_P_G = interp1(simData.tout,F_Drag_P_G',t);
F_Tethers_G = interp1(simData.tout,F_Tethers_G',t);
%% 4) Send to variable
estimationData.Ts = Ts; estimationData.Ts = Ts;
estimationData.m = m_P; estimationData.m = m_P;
estimationData.a_P = a_P_G; estimationData.a_P = a_P_G';
estimationData.F_Gravity = F_Gravity_G; estimationData.F_Gravity = F_Gravity_G;
estimationData.F_Drag_P = F_Drag_P_G; estimationData.F_Drag_P = F_Drag_P_G';
estimationData.F_Tethers = F_Tethers_G; estimationData.F_Tethers = F_Tethers_G';
estimationData.F_User = params.user_force; estimationData.F_User = params.user_force;
estimationData.T = simData.T; estimationData.T = t;
estimationData.num_points = num_points; estimationData.num_points = num_points;
end end
...@@ -49,9 +49,10 @@ function [params] = setUpParameters(traj_num) ...@@ -49,9 +49,10 @@ 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 = 100; int_limit = 1;
params.e_load_pos_Kp = 0; params.e_load_pos_Kp = 1;
params.e_load_pos_Ki = 0; params.e_load_pos_Ki = 0.1;
params.e_load_pos_Kd = 0.1;
params.e_load_pos_int_LL = [-int_limit;-int_limit;-int_limit]; 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_UL = [int_limit;int_limit;int_limit];
params.e_load_pos_int = zeros(3,1); params.e_load_pos_int = zeros(3,1);
...@@ -86,7 +87,7 @@ function [params] = setUpParameters(traj_num) ...@@ -86,7 +87,7 @@ function [params] = setUpParameters(traj_num)
end end
%% Set Wind Parameters %% Set Wind Parameters
params.wind.enable = 1; params.wind.enable = 0;
params.wind.speed = 0; % (m/s) params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
...@@ -119,8 +120,8 @@ function [params] = setUpParameters(traj_num) ...@@ -119,8 +120,8 @@ function [params] = setUpParameters(traj_num)
params = compute_quad_eq_state_and_controller(params); params = compute_quad_eq_state_and_controller(params);
%% User Force Input %% User Force Input
params.user_force_method = 1; params.user_force_method = 4;
params.user_force_amplitude = 5; % Newton's params.user_force_amplitude = 3; % Newton's
params.user_force = user_force_input(params); params.user_force = user_force_input(params);
end end
......
function [] = plot_vec3(T,v,name)
figure();
subplot(3,1,1);
plot(T,v(:,1));
ylabel('x-dir');
title(name);
subplot(3,1,2);
plot(T,v(:,2));
ylabel('y-dir');
subplot(3,1,3);
plot(T,v(:,3));
ylabel('z-dir');
xlabel('Time (s)');
end
...@@ -11,7 +11,7 @@ ...@@ -11,7 +11,7 @@
addpath(genpath(pwd)); addpath(genpath(pwd));
%% Set up Parameters %% Set up Parameters
traj_num = 2; traj_num = 1;
params = setUpParameters(traj_num); params = setUpParameters(traj_num);
%% Load Top Level Simulink Model (helps Simulink load ahead of time) %% Load Top Level Simulink Model (helps Simulink load ahead of time)
...@@ -20,12 +20,21 @@ open('coop_payload_sim'); ...@@ -20,12 +20,21 @@ open('coop_payload_sim');
%% Call Simulink and Run the Simulation %% Call Simulink and Run the Simulation
simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFinal)); simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFinal));
%% Haptic Force Estimation (post-process)
[params, simData] = hapticForceEstimation(params,simData);
%% Plot %% Plot
plot_vec3(simData.tout, simData.Xd_load,'Xd Load');
plot_vec3(simData.tout, simData.Xd_load_control,'Xd Load Control');
plot_vec3(simData.tout, simData.payload_X,'X Load');
% plot_position(simData.tout, simData.payload_X, simData.Xd_load_control);
% plot_position(simData.tout, simData.q1_X, simData.q1_Xd); % plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
% plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd); % plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd);
% plot_force(simData.tout, simData.q1_F_gravity, 'Q1 Gravity'); % plot_force(simData.tout, simData.q1_F_gravity, 'Q1 Gravity');
% plot_force(simData.tout, simData.q1_F_tether, 'Q1 Tether_G'); % plot_force(simData.tout, simData.q1_F_tether, 'Q1 Tether_G');
plot_force(simData.tout, simData.q1_F_drag, 'Q1 Drag_V'); % plot_force(simData.tout, simData.q1_F_drag, 'Q1 Drag_V');
%% Animate %% Animate
animate_simulink_data(simData, params); animate_simulink_data(simData, params);
...@@ -37,3 +46,5 @@ animate_simulink_data(simData, params); ...@@ -37,3 +46,5 @@ animate_simulink_data(simData, params);
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