From b49c95e7795ef6f343efacedb6b90a69c7855b32 Mon Sep 17 00:00:00 2001
From: Ugo Rosolia <ugo.rosolia@gmail.com>
Date: Thu, 1 Oct 2020 09:39:07 -0700
Subject: [PATCH] adding mpc

---
 src/main.py | 50 +++++++++++++++++++++++++-------------------------
 1 file changed, 25 insertions(+), 25 deletions(-)

diff --git a/src/main.py b/src/main.py
index b84686b..ff3918f 100644
--- a/src/main.py
+++ b/src/main.py
@@ -69,30 +69,30 @@ def main():
     # ======================================================================================================================
     # ======================================  LINEAR REGRESSION ============================================================
     # ======================================================================================================================
-    # print("Starting MPC")
-    # # Estimate system dynamics
-    # lamb = 0.0000001
-    # A, B, Error = Regression(xPID_cl, uPID_cl, lamb)
-    # mpcParam.A = A
-    # mpcParam.B = B
-    # # Initialize MPC and run closed-loop sim
-    # mpc = MPC(mpcParam)
-    # xMPC_cl, uMPC_cl, xMPC_cl_glob, _ = simulator.sim(xS, mpc)
-    # print("===== MPC terminated")
+    print("Starting MPC")
+    # Estimate system dynamics
+    lamb = 0.0000001
+    A, B, Error = Regression(xPID_cl, uPID_cl, lamb)
+    mpcParam.A = A
+    mpcParam.B = B
+    # Initialize MPC and run closed-loop sim
+    mpc = MPC(mpcParam)
+    xMPC_cl, uMPC_cl, xMPC_cl_glob, _ = simulator.sim(xS, mpc)
+    print("===== MPC terminated")
 
-    # # ======================================================================================================================
-    # # ===================================  LOCAL LINEAR REGRESSION =========================================================
-    # # ======================================================================================================================
-    # print("Starting TV-MPC")
-    # # Initialized predictive model
-    # predictiveModel = PredictiveModel(n, d, map, 1)
-    # predictiveModel.addTrajectory(xPID_cl,uPID_cl)
-    # #Initialize TV-MPC
-    # ltvmpcParam.timeVarying = True 
-    # mpc = MPC(ltvmpcParam, predictiveModel)
-    # # Run closed-loop sim
-    # xTVMPC_cl, uTVMPC_cl, xTVMPC_cl_glob, _ = simulator.sim(xS, mpc)
-    # print("===== TV-MPC terminated")
+    # ======================================================================================================================
+    # ===================================  LOCAL LINEAR REGRESSION =========================================================
+    # ======================================================================================================================
+    print("Starting TV-MPC")
+    # Initialized predictive model
+    predictiveModel = PredictiveModel(n, d, map, 1)
+    predictiveModel.addTrajectory(xPID_cl,uPID_cl)
+    #Initialize TV-MPC
+    ltvmpcParam.timeVarying = True 
+    mpc = MPC(ltvmpcParam, predictiveModel)
+    # Run closed-loop sim
+    xTVMPC_cl, uTVMPC_cl, xTVMPC_cl_glob, _ = simulator.sim(xS, mpc)
+    print("===== TV-MPC terminated")
 
     # ======================================================================================================================
     # ==============================  LMPC w\ LOCAL LINEAR REGRESSION ======================================================
@@ -128,8 +128,8 @@ def main():
 
     print("===== Start Plotting")
     plotTrajectory(map, xPID_cl, xPID_cl_glob, uPID_cl, 'PID')
-    # plotTrajectory(map, xMPC_cl, xMPC_cl_glob, uMPC_cl, 'MPC')
-    # plotTrajectory(map, xTVMPC_cl, xTVMPC_cl_glob, uTVMPC_cl, 'TV-MPC')
+    plotTrajectory(map, xMPC_cl, xMPC_cl_glob, uMPC_cl, 'MPC')
+    plotTrajectory(map, xTVMPC_cl, xTVMPC_cl_glob, uTVMPC_cl, 'TV-MPC')
     plotClosedLoopLMPC(lmpc, map)
     animation_xy(map, lmpc, Laps-1)
     plt.show()