results

k-sheridan · Apr 29, 2018 · 0730a15 · 0730a15
1 parent 2d1b707
commit 0730a15
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 20 deletions.
diff --git a/matlab/TrajectoryGenerator/final_trajectory.fig b/matlab/TrajectoryGenerator/final_trajectory.fig
diff --git a/matlab/TrajectoryGenerator/generateTrajectory.m b/matlab/TrajectoryGenerator/generateTrajectory.m
@@ -1,5 +1,40 @@
-start = [0, 0, 0, 0, 0;0, 0, 0, 0, 0;0, 0, 0, 0, 0];
-mid = [5; 2;2.5;1; -5; -3]
-final = [10, 0, 0, 0, 0;5, 0, 0, 0, 0; 0.3, 0, 0, 0, 0]
+start = [0, 0, 0, 0, 0;0, 0, 0, 0, 0;2, 0, 0, 0, 0];
+mid = [-5; -3;2.5;0; -5; 2.5; 10; 0; 2.5; 5; 5; 2.5];
+final = [10, 0, 0, 0, 0;10, 0, 0, 0, 0; 2, 0, 0, 0, 0]
 
-[traj, flightTime] = minimumTimeTrajectoryGenerator(start, mid, final, 'VEL', 5, [1.5, 1.5, 1], 10, -45, 100, pi/6)
+
+[traj, flightTime] = minimumTimeTrajectoryGenerator(start, mid, final, 'VEL', 5, [1.5, 1.5, 1], 10, -45, 100, pi/6, 1)
+
+%plot the trajectory
+clf;
+[p1, p2] = trajectoryPlotter(traj);
+daspect([5 5 5])
+xlim([-10, 15])
+ylim([-10, 15])
+zlim([-10, 10])
+hold on
+arrow3(p1, p2, 'b', 0.4)
+title('Initial Trajectory')
+xlabel('x (m)')
+ylabel('y (m)')
+grid on;
+savefig('initial_trajectory');
+hold off
+
+[traj, flightTime] = minimumTimeTrajectoryGenerator(start, mid, final, 'VEL', 5, [1.5, 1.5, 1], 10, -45, 100, pi/6, 10)
+
+%plot the trajectory
+clf;
+[p1, p2] = trajectoryPlotter(traj);
+daspect([5 5 5])
+xlim([-10, 15])
+ylim([-10, 15])
+zlim([-10, 10])
+hold on
+arrow3(p1, p2, 'b', 0.4)
+title('Final Trajectory')
+xlabel('x (m)')
+ylabel('y (m)')
+grid on;
+savefig('final_trajectory');
+hold off
diff --git a/matlab/TrajectoryGenerator/initial_trajectory.fig b/matlab/TrajectoryGenerator/initial_trajectory.fig
diff --git a/matlab/TrajectoryGenerator/minimumTimeTrajectoryGenerator.m b/matlab/TrajectoryGenerator/minimumTimeTrajectoryGenerator.m
@@ -1,4 +1,4 @@
-function [ Trajectory, totalFlightTime ] = minimumTimeTrajectoryGenerator( start, mid, final, MIDPOINT_MODE, Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle)
+function [ Trajectory, totalFlightTime ] = minimumTimeTrajectoryGenerator( start, mid, final, MIDPOINT_MODE, Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle, ITERATIONS)
 %UNTITLED7 Summary of this function goes here
 %   This will generate a trajectory that is fly-able by a quadrotor it may
 %   take a second.
@@ -19,21 +19,24 @@
 [~, midCols] = size(mid);
 Trajectory = zeros(3, 11, midCols + 1);
 
+
 %if this is in velocity mode
 if strcmp(MIDPOINT_MODE, 'VEL') == 1
 
     if midCols ~= 0
-        Trajectory(:, :, 1) = polynomialTrajectorySolver([start(1, :), mid(1, 1), mid(4, 1), 0, 0, 0], [start(2, :), mid(2, 1), mid(5, 1), 0, 0, 0], [start(3, :), mid(3, 1), mid(6, 1), 0, 0, 0], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle);
+
+
+        Trajectory(:, :, 1) = polynomialTrajectorySolver([start(1, :), mid(1, 1), mid(4, 1), 0, 0, 0], [start(2, :), mid(2, 1), mid(5, 1), 0, 0, 0], [start(3, :), mid(3, 1), mid(6, 1), 0, 0, 0], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle, ITERATIONS);
 
         for z_index = (1:1:midCols);
             if ~(z_index == midCols)
-                Trajectory(:, :, z_index + 1) = polynomialTrajectorySolver([mid(1, z_index), mid(4, z_index), 0, 0, 0, mid(1, z_index + 1), mid(4, z_index + 1), 0, 0, 0], [mid(2, z_index), mid(5, z_index), 0, 0, 0, mid(2, z_index + 1), mid(5, z_index + 1), 0, 0, 0], [mid(3, z_index), mid(6, z_index), 0, 0, 0, mid(3, z_index + 1), mid(6, z_index + 1), 0, 0, 0], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle);
+                Trajectory(:, :, z_index + 1) = polynomialTrajectorySolver([mid(1, z_index), mid(4, z_index), 0, 0, 0, mid(1, z_index + 1), mid(4, z_index + 1), 0, 0, 0], [mid(2, z_index), mid(5, z_index), 0, 0, 0, mid(2, z_index + 1), mid(5, z_index + 1), 0, 0, 0], [mid(3, z_index), mid(6, z_index), 0, 0, 0, mid(3, z_index + 1), mid(6, z_index + 1), 0, 0, 0], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle, ITERATIONS);
             else
-                Trajectory(:, :, z_index + 1) = polynomialTrajectorySolver([mid(1, z_index), mid(4, z_index), 0, 0, 0, final(1, :)], [mid(2, z_index), mid(5, z_index), 0, 0, 0, final(2, :)], [mid(3, z_index), mid(6, z_index), 0, 0, 0, final(3, :)], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle);
+                Trajectory(:, :, z_index + 1) = polynomialTrajectorySolver([mid(1, z_index), mid(4, z_index), 0, 0, 0, final(1, :)], [mid(2, z_index), mid(5, z_index), 0, 0, 0, final(2, :)], [mid(3, z_index), mid(6, z_index), 0, 0, 0, final(3, :)], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle, ITERATIONS);
             end
         end
     else
-        Trajectory(:, :, 1) = polynomialTrajectorySolver([start(1, :), final(1, :)], [start(2, :), final(2, :)], [start(3, :), final(3, :)], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle);
+        Trajectory(:, :, 1) = polynomialTrajectorySolver([start(1, :), final(1, :)], [start(2, :), final(2, :)], [start(3, :), final(3, :)], Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle, ITERATIONS);
     end
 
 elseif strcmp(MIDPOINT_MODE, 'NO_VEL') == 1
@@ -44,14 +47,5 @@
 
 totalFlightTime = sum(Trajectory(1, 11, :));
 
-%plot the trajectory
-%[p1, p2] = trajectoryPlotter(Trajectory);
-
-%daspect([5 5 5])
-%axis([-1 11 -1 11 -10 10])
-%hold on
-%arrow3(p1, p2, 'b', 0.4)
-%hold off
-
 end
 
diff --git a/matlab/TrajectoryGenerator/polynomialTrajectorySolver.m b/matlab/TrajectoryGenerator/polynomialTrajectorySolver.m
@@ -1,12 +1,11 @@
-function [C] = polynomialTrajectorySolver(X, Y, Z, Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle)
+function [C] = polynomialTrajectorySolver(X, Y, Z, Mass, Moment, MaxVel, MinZForce, MaxForce, MaxAngle, ITERATIONS)
 %UNTITLED Summary of this function goes here
 %   Detailed explanation goes here
 %   This will solve for the coefficients of a constrained 9 order
 %   polynomial
 % END_POINT_MODES: 'FULL', 'VEL', 'NOVEL'
 % for vel and no vel use zeros in their place
 %the number of iterations
-ITERATIONS = 10;
 
 %declare ti as zero
 ti = 0;

diff --git a/matlab/TrajectoryGenerator/trajectoryPlotter.m b/matlab/TrajectoryGenerator/trajectoryPlotter.m
@@ -10,12 +10,17 @@
 pAccel = [polyval(polyder(Traj(1, 1:10, 1)), (time:0.1:time + Traj(1, 11, 1)))', polyval(polyder(Traj(2, 1:10, 1)), (time:0.1:time + Traj(1, 11, 1)))', polyval(polyder(Traj(3, 1:10, 1)), (time:0.1:time + Traj(1, 11, 1)))'];
 time = time + Traj(1, 11, 1);
 
+dt_vec = [];
+
 for z_index = (2:1:trajZ)
     p1 = [p1; [polyval(Traj(1, 1:10, z_index), (0:0.1:Traj(1, 11, z_index)))', polyval(Traj(2, 1:10, z_index), (0:0.1:Traj(1, 11, z_index)))', polyval(Traj(3, 1:10, z_index), (0:0.1:Traj(1, 11, z_index)))']];
     pAccel = [pAccel; [polyval((polyder(Traj(1, 1:10, z_index))), (0:0.1:Traj(1, 11, z_index)))', polyval((polyder(Traj(2, 1:10, z_index))), (0:0.1:Traj(1, 11, z_index)))', polyval((polyder(Traj(3, 1:10, z_index))), (0:0.1:Traj(1, 11, z_index)))']];
     time = time + Traj(1, 11, z_index);
+    dt_vec = [dt_vec, Traj(1, 11, z_index)];
 end
 
+dt_vec
+
 p2 = p1 + (pAccel * 0.4);
 
 end