Adding plotting.

Guidance/user_force_input.m

@@ -5,7 +5,9 @@ function [F_user] = user_force_input(params)
     %% Setup
     num_points = size(params.payload.xd,2);
     first_quarter_point = floor(num_points / 4);
+    one_third_pt = floor(num_points / 3);
     halfway_point = floor(num_points / 2);
+    two_third_pt = floor(2 * num_points / 3);
     three_quarters_point = floor(num_points * 3 / 4);
     F_user = zeros(3,num_points);
     
@@ -31,6 +33,19 @@ function [F_user] = user_force_input(params)
             F_user(1,first_quarter_point:end) = params.user_force_amplitude;
             F_user(1,halfway_point:end) = 0;
         
+        case 7 % Ramp Up, Constant, Ramp Down
+            % Ramp Up
+            inds = first_quarter_point:one_third_pt;
+            F_user(1,inds) = params.user_force_amplitude .*(inds - inds(1))./(length(inds));
+            
+            % Constant
+            inds = one_third_pt:two_third_pt;
+            F_user(1,inds) = ones(1,length(inds)) .* params.user_force_amplitude;
+            
+            % Ramp Down
+            inds = two_third_pt:three_quarters_point;
+            F_user(1,inds) = params.user_force_amplitude .*(inds(end) - inds)./(length(inds));
+            
     end
     
 %     %% Double Check Waveform

Parameters/setUpParameters.m

@@ -127,7 +127,7 @@ function [params] = setUpParameters(traj_num)
     params = compute_quad_guidance_2(params);
     
     %% User Force Input
-    params.user_force_method = 6;%6;
+    params.user_force_method = 7;%6;
     params.user_force_amplitude = 0.5; % Newton's
     params.user_force = user_force_input(params);
     

Plotting/helper_functions/plot_position.m

-function [] = plot_position(T,X,Xd,title_string,location)
+function [f] = plot_position(T,X,Xd,title_string,location)
 
-    figure();
+    f = figure();
 
     subplot(3,1,1);
     plot(T,X(:,1));
@@ -9,7 +9,7 @@ function [] = plot_position(T,X,Xd,title_string,location)
     hold off;
     ylabel('x (m)');
     title(title_string);
-    legend('Actual','Desired');
+    legend('Actual','Desired','Location','Best');
 
     subplot(3,1,2);
     plot(T,X(:,2));
@@ -17,7 +17,7 @@ function [] = plot_position(T,X,Xd,title_string,location)
     plot(T,Xd(:,2),'r--');
     hold off;
     ylabel('y (m)');
-    legend('Actual','Desired');
+    legend('Actual','Desired','Location','Best');
 
     subplot(3,1,3);
     plot(T,X(:,3));
@@ -26,7 +26,7 @@ function [] = plot_position(T,X,Xd,title_string,location)
     hold off;
     ylabel('z (m)');
     xlabel('Time (s)');
-    legend('Actual','Desired');
+    legend('Actual','Desired','Location','Best');
     
     movegui(location);
 

Plotting/paper_plot_functions/paper_plot_admittance_control.m

+function [] = paper_plot_admittance_control(d)
+%PAPER_PLOT_ADMITTANCE_CONTROL
+
+    %% 1) Payload Position
+    f = plot_position(d.tout, d.payload_X, d.Xd_load,'','northwest');
+    
+    % Adjust ylims
+    extra_ylims = [-0.05, 0.05];
+    f.Children(6).YLim = f.Children(6).YLim + extra_ylims; %x
+    f.Children(4).YLim = f.Children(4).YLim + extra_ylims; %y
+    f.Children(2).YLim = f.Children(2).YLim + extra_ylims; %z
+    
+    % Adjust font size and boldness
+    
+    % Adjust line widths
+    
+    % Save the plot
+    
+    %% 2) Payload Velocity
+    plot_velocity(d.tout, d.payload_X, d.Xd_load,'','northeast');
+
+    %% 3) Haptic Force Estimate
+    plotHapticForceEstimate(d,'southwest');
+
+
+end
+

Plotting/saved_plots/.gitignore

+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
\ No newline at end of file

coopPayloadSim.m

@@ -23,11 +23,13 @@ simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFina
 %% Haptic Force Estimation (post-process)
 % [params, simData] = hapticForceEstimation(params,simData);
 
-%% Plot
-plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position','northwest');
-plot_velocity(simData.tout, simData.payload_X, simData.Xd_load,'Payload Velocity','northeast');
+%% Plot (Animation is always north, do the 5 other directions
+paper_plot_admittance_control(simData);
 
-plotHapticForceEstimate(simData,'northeast');
+% plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position','northwest');
+% 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');
 %