Things are working! I think I'm done with basic simulations

Parameters/setUpParameters.m

@@ -61,7 +61,10 @@ function [params] = setUpParameters(traj_num)
     r = params.payload.diameter / 2;
 
     %% Adjust Payload Controller Parameters
-    params.e_load_pos_Kp = [diag([2, 2, 2]), diag([0.7, 0.7, 0.7])];
+    params.e_load_pos_Kp = [diag([0.7, 0.7, 0.7]), diag([0.2, 0.2, 0.2])];
+%     params.e_load_pos_Kp = [diag([2, 2, 2]), diag([0.7, 0.7, 0.7])];
+%       params.e_load_pos_Kp = [diag([1, 1, 1]), diag([0.1, 0.1, 0.1])];
+
 
     
     %% Load Multirotors
@@ -124,7 +127,7 @@ function [params] = setUpParameters(traj_num)
     params = compute_quad_guidance_2(params);
     
     %% User Force Input
-    params.user_force_method = 1;%6;
+    params.user_force_method = 6;%6;
     params.user_force_amplitude = 0.5; % Newton's
     params.user_force = user_force_input(params);
     
@@ -134,9 +137,10 @@ function [params] = setUpParameters(traj_num)
     params.admittance_controller.lpf_cutoff_freq = 2*pi*5; 
     
     %% Admittance Controller
-    params.admittance_controller.en = 0; % 0 = False, 1 = True
+    params.admittance_controller.en = 1; % 0 = False, 1 = True
     params.admittance_controller.c = 5;
     params.admittance_controller.m = 0.1;
+    params.admittance_controller.turn_on_force = 0.1;
 
     
     

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)
@@ -27,13 +27,13 @@ simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFina
 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');
+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_velocity(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Vel','northeast');
-plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Attitude','north');
+% plot_position(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Position','southeast');
+% plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Vel','northeast');
+% plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd,'Quad 1 Attitude','north');
 
 
 % plot_position(simData.tout, simData.q1_X, simData.q1_Xd);