Square and hover trajectory working, probably final change

Guidance/splineTogether.m

@@ -43,8 +43,8 @@ function [WPs] = splineTogether(coarseWPs, dt)
     end
     
 %% 3) Combine Data into a single WPs variable
-    %WPs = [x_all', y_all', z_all', vx_all', vy_all', vz_all'];
-    WPs = [x_all', y_all', z_all', zeros(size(x_all', 1), 1), zeros(size(x_all', 1), 1), zeros(size(x_all', 1), 1)];
+    WPs = [x_all', y_all', z_all', vx_all', vy_all', vz_all'];
+    %WPs = [x_all', y_all', z_all', zeros(size(x_all', 1), 1), zeros(size(x_all', 1), 1), zeros(size(x_all', 1), 1)];
     
 end
 

Parameters/setUpParameters_OneQuad.m

@@ -77,7 +77,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
     %% Offset in position
     %params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.1;0.1;0.1];
     %params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.1; 0.1; 0.1]; %10 cm offset
-    params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.2;0.2; 0.2]; %20 cm offset 
+    %params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.2;0.2; 0.2]; %20 cm offset 
     %params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.22;0.22; 0.22]; %22 cm offset
 
     %%Simulation starts breaking at an offset of 23 cm

singleVehicleSim.m

@@ -8,7 +8,7 @@
 addpath(genpath(pwd));
 
 %% Set up Parameters
-traj_num = 1;
+traj_num = 2;
 params = setUpParameters_OneQuad(traj_num);
 params.num_vehicles = 1;