Commit ea8401d6 authored by mmroma's avatar mmroma
Browse files

In process of making admittance control work.

parent 2230dbf3
function [params] = compute_quad_eq_state_and_controller(params)
% compute_quad_eq_state_and_controller
%% What we have from guidance
% params.multirotors{i}.xd
% params.F_Tether_i_P
% params.s_hat_i_P
% params.l_d_i
% params.s_i_P
% params.f_d_i = f_d_i;
% params.f_thrust_i = f_thrust_i;
% params.phi_i = phi_i;
% params.theta_i = theta_i;
%%
end
......@@ -6,12 +6,14 @@ function [params, simData] = hapticForceEstimation(params,simData)
%% Level 0) Direct Haptic Force Calculation w/ No Filter
F_User_est0 = direcHapticForceCalc(estimationData);
plotHapticForceEstimate(estimationData, F_User_est0);
%% Level 1) Kalman Filter Haptic Force Calculation
[F_User_est1, F_User_est1_Sigma] = kalmanFilterHapticForceCalc(estimationData);
plotHapticForceEstimate(estimationData, F_User_est1);
plotHapticForceEstimateUncertainty(estimationData, F_User_est1_Sigma);
% [F_User_est1, F_User_est1_Sigma] = kalmanFilterHapticForceCalc(estimationData);
% plotHapticForceEstimate(estimationData, F_User_est1);
% plotHapticForceEstimateUncertainty(estimationData, F_User_est1_Sigma);
%% Save Data to simData
simData.estimationData = estimationData;
simData.estimationData.F_User_est0 = F_User_est0;
end
......@@ -44,9 +44,9 @@ function [F_User_est, F_User_est_Sigma] = kalmanFilterHapticForceCalc(data)
u_t = data.F_Gravity;
for i=1:data.num_points
if i==499
disp("Help");
end
% if i==499
% disp("Help");
% end
% Predict
Mu_t1_hat = A * Mu_t + B * u_t;
......
function [] = plotHapticForceEstimate(data, F_User_est)
function [] = plotHapticForceEstimate(data, location)
%plotHapticForceEstimate
%% 4) Plot Results
figure();
dims = {'X','Y','Z'};
for i=1:3
subplot(3,1,i);
plot(data.T,F_User_est(i,:));
plot(data.tout,data.F_user_est(:,i));
hold on;
plot(data.T,data.F_User(i,:));
plot(data.tout,data.F_user(:,i));
% plot(data.tout,data.F_user_est_unfilt(:,i),'--');
hold off;
legend('Estimate','Actual');
% legend('Estimate','Actual','Unfiltered Estimate');
xlabel('Time (s)');
ylabel('Force (N)');
title([dims{i} ' Direction - Haptic Force Input']);
end
movegui(location);
% figure(2);
% dims = {'X','Y','Z'};
% for i=1:3
......
......@@ -23,7 +23,7 @@ function [estimationData] = preProcessHapticForceEstimation(params,simData)
F_Tethers_G = zeros(3,num_points);
simData.F_tethers_P
% 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_P(i,:)';
......
......@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover()
%WPs_set1
h = 1.5;
coarse_WPs = [0, 0, h, 0;
0, 0, h, 20];
0, 0, h, 10];
end
......@@ -4,7 +4,9 @@ function [F_user] = user_force_input(params)
%% Setup
num_points = size(params.payload.xd,2);
first_quarter_point = floor(num_points / 4);
halfway_point = floor(num_points / 2);
three_quarters_point = floor(num_points * 3 / 4);
F_user = zeros(3,num_points);
%%
......@@ -24,6 +26,11 @@ function [F_user] = user_force_input(params)
case 5 % Ramp
inds = halfway_point:num_points;
F_user(1,halfway_point:end) = params.user_force_amplitude .*(inds - halfway_point)./(num_points - halfway_point);
case 6 % Step Up and Down
F_user(1,first_quarter_point:end) = params.user_force_amplitude;
F_user(1,halfway_point:end) = 0;
end
% %% Double Check Waveform
......
......@@ -22,21 +22,33 @@ function [params] = setUpParameters(traj_num)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
%% Disturbance Forces & Torques
quads_dist_F_power = 1e-3;
quads_dist_F_power = 0;
% quads_dist_F_power = 1e-3;
quads_dist_M_power = 0;
load_dist_F_power = 1e-3;
load_dist_F_power = 0;
% load_dist_F_power = 1e-3;
load_dist_M_power = 0;
%% Sensor Noise
quad_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-6;
ones(3,1).* 1e-6;
ones(3,1).* 1e-4];
load_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-6;
ones(3,1).* 1e-6;
ones(3,1).* 1e-4];
% quad_noise_power = [ones(3,1).* 1e-8;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6];
%
% load_noise_power = [ones(3,1).* 1e-8;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6;
% ones(3,1).* 1e-6];
quad_noise_power = [ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0];
load_noise_power = [ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0;
ones(3,1).* 0];
%% Load Payload Parameters
params.payload = parameters_Payload(params);
......@@ -49,10 +61,11 @@ function [params] = setUpParameters(traj_num)
r = params.payload.diameter / 2;
%% Adjust Payload Controller Parameters
int_limit = 1;
params.e_load_pos_Kp = 1;
params.e_load_pos_Ki = 0.1;
params.e_load_pos_Kd = 0.1;
int_limit = 1; % meters
% params.e_load_pos_Kp = diag([1,1,1]);
params.e_load_pos_Kp = diag([0.2,0.2,1]);
params.e_load_pos_Ki = diag([0.01,0.01,0.1]);
% params.e_load_pos_Kd = diag([0,0,0]);;
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);
......@@ -116,13 +129,22 @@ function [params] = setUpParameters(traj_num)
params.payload.x0 = params.payload.xd(:,1);
params = compute_quad_guidance_2(params);
%% Adjust Quad Control Based on Payload Trajectory
params = compute_quad_eq_state_and_controller(params);
%% User Force Input
params.user_force_method = 4;
params.user_force_amplitude = 3; % Newton's
params.user_force_method = 1;%6;
params.user_force_amplitude = 0.5; % Newton's
params.user_force = user_force_input(params);
%% Haptic Force Estimation
params.admittance_controller.acc_noise_power = ones(3,1).* 1e-4;
params.admittance_controller.acc_noise_seed = [477;478;479];
params.admittance_controller.lpf_cutoff_freq = 2*pi*5;
%% Admittance Controller
params.admittance_controller.en = 0; % 0 = False, 1 = True
params.admittance_controller.c = 5;
params.admittance_controller.m = 0.1;
end
function [] = plot_position(T,X,Xd)
function [] = plot_position(T,X,Xd,title_string,location)
figure();
......@@ -8,7 +8,7 @@ function [] = plot_position(T,X,Xd)
plot(T,Xd(:,1),'r--');
hold off;
ylabel('x (m)');
title('Position');
title(title_string);
legend('Actual','Desired');
subplot(3,1,2);
......@@ -28,5 +28,7 @@ function [] = plot_position(T,X,Xd)
xlabel('Time (s)');
legend('Actual','Desired');
movegui(location);
end
function [] = plot_vec3(T,v,name)
function [] = plot_vec3(T,v,name,location)
figure();
subplot(3,1,1);
......@@ -15,12 +15,6 @@ function [] = plot_vec3(T,v,name)
ylabel('z-dir');
xlabel('Time (s)');
if strcmp(name, 'Xd Load')
movegui('southwest');
elseif strcmp(name, 'Xd Load Control')
movegui('south');
elseif strcmp(name, 'X Load')
movegui('southeast');
end
movegui(location);
end
......@@ -11,7 +11,7 @@
addpath(genpath(pwd));
%% Set up Parameters
traj_num = 1;
traj_num = 2;
params = setUpParameters(traj_num);
%% Load Top Level Simulink Model (helps Simulink load ahead of time)
......@@ -21,15 +21,18 @@ open('coop_payload_sim');
simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFinal));
%% Haptic Force Estimation (post-process)
[params, simData] = hapticForceEstimation(params,simData);
% [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,'Payload Position','northwest');
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');
plot_position(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Position','southeast');
% 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');
......
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