Commit c65f434f authored by mmroma's avatar mmroma
Browse files

Adding plotting.

parent 60541b9a
......@@ -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
......
......@@ -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);
......
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);
......
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
# Ignore everything in this directory
*
# Except this file
!.gitignore
\ No newline at end of file
......@@ -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');
%
......
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