More outputs / debugging info

- Decrease `sim_stop_idx` to 1500
- Add some debugging messages for when simulation hits
  limit or when `fmincon` can't satisfy constraints
- Add more `fmincon` outputs for future debugging
- Add obstacle information to `getTrajectoryInfo` call
  in `part2_test_controller.m` to see when car crashes

Deliverables/MPC_Class.m

@@ -56,7 +56,7 @@ classdef MPC_Class
         % Simulation Parameters
         T_s = 0.01;           % Step Size [s]
         T_p = 0.5;            % Prediction Horizon [s]
-        sim_stop_idx = 2e3; % Index in reference trajectory to stop sim
+        sim_stop_idx = 1.5e3; % Index in reference trajectory to stop sim
 
         % Decision Variables
         nstates = 6; % Number of states per prediction
@@ -130,6 +130,7 @@ classdef MPC_Class
             % Stop simulation when `sim_stop_idx` is reached
             obj.FLAG_terminate = 0;
             if obj.ref_idx > obj.sim_stop_idx
+                disp('Reached simulation stop index')
                 obj.FLAG_terminate = 1;
             end
         end
@@ -146,8 +147,13 @@ classdef MPC_Class
             [Aeq, beq] = obj.eq_cons(A, B);
             [Lb, Ub]   = obj.bound_cons();
 
-            Z_err = ...
-                fmincon( ...
+            [ ...          % `fmincon` outputs
+             Z_err, ...    % x:        Solution
+             J_val, ...    % fval:     Objective function value at solution
+             exitflag, ... % exitflag: Reason `fmincon` stopped
+             output, ...   % output:   Information about the optimization process
+            ] = ...
+                fmincon( ...           % `fmincon` inputs
                     @obj.cost_fun, ... % fun:     Function to minimize
                     Z_err_init, ...    % x0:      Initial point
                     [], ...            % A:       Linear inequality constraints
@@ -160,6 +166,11 @@ classdef MPC_Class
                     obj.options ...    % options: Optimization options
             );
 
+            if exitflag == -2
+                disp('No feasible point was found')
+                obj.FLAG_terminate = 1;
+            end
+
             % NOTE: Error = Actual - Reference
             %    => Actual = Error + Reference
             Z_ref = obj.get_ref_decision_variable();

Experimentation/part2_test_controller.m

@@ -5,7 +5,7 @@ clc;
 
 %% Call simulation functions
 [Y,U,t_total,t_update,Xobs] = forwardIntegrate();
-info = getTrajectoryInfo(Y,U)
+info = getTrajectoryInfo(Y,U,Xobs)
 
 %% Figures
 % Plot segmented trajectory for debugging purposes