From 69c704e66bb4ee94f0119b4213534f97b3964b3f Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Sun, 12 Dec 2021 17:31:52 -0500
Subject: [PATCH] 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                | 21 ++++++++++++++++-----
 Experimentation/part2_test_controller.m |  2 +-
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/Deliverables/MPC_Class.m b/Deliverables/MPC_Class.m
index 15bddc2..638e0b4 100644
--- a/Deliverables/MPC_Class.m
+++ b/Deliverables/MPC_Class.m
@@ -54,9 +54,9 @@ 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
+        T_s = 0.01;           % Step Size [s]
+        T_p = 0.5;            % Prediction Horizon [s]
+        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();
diff --git a/Experimentation/part2_test_controller.m b/Experimentation/part2_test_controller.m
index 3eaebed..d41d313 100644
--- a/Experimentation/part2_test_controller.m
+++ b/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