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)
Ts = params.multirotors{1}.ctlDt;
%% 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;
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);
a_P_G = zeros(3,num_points);
......@@ -19,9 +21,12 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData)
% Tether Forces
F_Tethers_G = zeros(3,num_points);
simData.F_tethers_P
for i=1:num_points
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
......@@ -32,18 +37,27 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData)
F_Drag_P_G = zeros(3,num_points);
for i=1:num_points
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
%% 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.m = m_P;
estimationData.a_P = a_P_G;
estimationData.a_P = a_P_G';
estimationData.F_Gravity = F_Gravity_G;
estimationData.F_Drag_P = F_Drag_P_G;
estimationData.F_Tethers = F_Tethers_G;
estimationData.F_Drag_P = F_Drag_P_G';
estimationData.F_Tethers = F_Tethers_G';
estimationData.F_User = params.user_force;
estimationData.T = simData.T;
estimationData.T = t;
estimationData.num_points = num_points;
end
......@@ -49,9 +49,10 @@ function [params] = setUpParameters(traj_num)
r = params.payload.diameter / 2;
%% Adjust Payload Controller Parameters
int_limit = 100;
params.e_load_pos_Kp = 0;
params.e_load_pos_Ki = 0;
int_limit = 1;
params.e_load_pos_Kp = 1;
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_UL = [int_limit;int_limit;int_limit];
params.e_load_pos_int = zeros(3,1);
......@@ -86,7 +87,7 @@ function [params] = setUpParameters(traj_num)
end
%% Set Wind Parameters
params.wind.enable = 1;
params.wind.enable = 0;
params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
......@@ -119,8 +120,8 @@ function [params] = setUpParameters(traj_num)
params = compute_quad_eq_state_and_controller(params);
%% User Force Input
params.user_force_method = 1;
params.user_force_amplitude = 5; % Newton's
params.user_force_method = 4;
params.user_force_amplitude = 3; % Newton's
params.user_force = user_force_input(params);
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 @@
addpath(genpath(pwd));
%% Set up Parameters
traj_num = 2;
traj_num = 1;
params = setUpParameters(traj_num);
%% Load Top Level Simulink Model (helps Simulink load ahead of time)
......@@ -20,12 +20,21 @@ open('coop_payload_sim');
%% Call Simulink and Run the Simulation
simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFinal));
%% Haptic Force Estimation (post-process)
[params, simData] = hapticForceEstimation(params,simData);
%% 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_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_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_simulink_data(simData, params);
......@@ -37,3 +46,5 @@ animate_simulink_data(simData, params);
Markdown is supported
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