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 :)

Estimation/preProcessHapticForceEstimation.m

@@ -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
 

Parameters/setUpParameters.m

@@ -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

Plotting/helper_functions/plot_vec3.m

+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
+

coopPayloadSim.m

@@ -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);
 
 
 
+    
+