Compute quad guidance in real time now. Also adding velocity gain to payload controller.

Guidance/compute_quad_guidance_2.m

@@ -86,7 +86,9 @@ function [params] = compute_quad_guidance_2(params)
         rpy = zeros(6,size(r_dot_d_Vi_G,2));
         rpy(1:2,:) = [phi_i{i}; theta_i{i}] * ones(1,size(r_dot_d_Vi_G,2));
         
-        quads_xd{i} = [r_d_Vi_G; r_dot_d_Vi_G; rpy];
+%         quads_xd{i} = [r_d_Vi_G; r_dot_d_Vi_G; rpy];
+        
+        quads_xd{i} = [r_d_Vi_G; zeros(3,size(params.payload.xd,2)); rpy];
     end
     
     

Parameters/setUpParameters.m

@@ -63,12 +63,14 @@ function [params] = setUpParameters(traj_num)
     %% Adjust Payload Controller Parameters
     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_Kp = diag([0.2,0.2,1]);
+    params.e_load_pos_Kp = diag([0.0,0.0,0.0, 0.0,0.0,0.0]);
+%     params.e_load_pos_Ki = diag([0.01,0.01,0.1]);
+    params.e_load_pos_Ki = diag([0.0,0.0,0.1, 0.0,0.0,0.0]);
 %     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);
+    params.e_load_int_LL = -int_limit.*ones(6,1);
+    params.e_load_int_UL = int_limit.*ones(6,1);
+%     params.e_load_pos_int = zeros(3,1);
     
     %% Load Multirotors
     params.num_vehicles = 5;

Plotting/helper_functions/plot_orientation.m

-function [] = plot_orientation(T,X,Xd, simData)
+function [] = plot_orientation(T,X,Xd,title_string,location)
 
     figure();
 
     subplot(3,1,1);
     plot(T,X(:,7));
     hold on;
-    plot(T,simData.q1_phi_des,'r--');
+    plot(T,Xd(:,7),'r--');
     hold off;
     ylabel('\phi (rad)');
     xlabel('Time (s)');
-    title('Attitude');
+    title(title_string);
     legend('Actual','Desired');
    % ylim([-pi/2 pi/2]);   
     
@@ -27,7 +27,7 @@ function [] = plot_orientation(T,X,Xd, simData)
     subplot(3,1,2);
     plot(T,X(:,8));
     hold on;
-    plot(T,simData.q1_theta_des,'r--');
+    plot(T,Xd(:,8),'r--');
     hold off;
     ylabel('\theta (rad)');
     xlabel('Time (s)');
@@ -43,6 +43,8 @@ function [] = plot_orientation(T,X,Xd, simData)
     xlabel('Time (s)');
   %  ylim([-pi/2 pi/2]);
     legend('Actual','Desired');
+    
+    movegui(location);
 
 end
 

Plotting/helper_functions/plot_velocity.m

-function [] = plot_velocity(T,X,Xd, simData)
+function [] = plot_velocity(T,X,Xd,title_string,location)
 
     figure();
 
     subplot(3,1,1);
     plot(T,X(:,4));
     hold on;
-    plot(T,simData.q1_x_dot_des,'r--');
+    plot(T,Xd(:,4),'r--');
     hold off;
     ylabel('x_{dot} (m/s)');
-    title('Velocity');
+    title(title_string);
     legend('Actual','Desired');
 
     subplot(3,1,2);
     plot(T,X(:,5));
     hold on;
-    plot(T,simData.q1_y_dot_des,'r--');
+    plot(T,Xd(:,5),'r--');
     hold off;
     ylabel('y_{dot} (m/s)');
     legend('Actual','Desired');
@@ -22,11 +22,12 @@ function [] = plot_velocity(T,X,Xd, simData)
     subplot(3,1,3);
     plot(T,X(:,6));
     hold on;
-    plot(T,simData.q1_z_dot_des,'r--');
+    plot(T,Xd(:,6),'r--');
     hold off;
     ylabel('z_{dot} (m/s)');
     xlabel('Time (s)');
     legend('Actual','Desired');
 
+    movegui(location);
 end
 

coopPayloadSim.m

@@ -25,12 +25,15 @@ simData = sim('coop_payload_sim','StartTime','0','StopTime',num2str(params.tFina
 
 %% Plot
 plot_position(simData.tout, simData.payload_X, simData.Xd_load,'Payload Position','northwest');
-plotHapticForceEstimate(simData,'northeast');
+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');
+% 
 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);