feat(t2-p5): change dt, upper bound for steering

- add comments from project review

term-2/p5-model-predictive-control/src/MPC.cpp

@@ -13,7 +13,7 @@ using CppAD::AD;
 // TODO: Set the timestep length and duration
 // size_t: type returned by sizeof, widely used to represent sizes and counts
 size_t N = 10;
-double dt = 0.2;
+double dt = 0.1;
 
 // Set cost factors
 // TODO: tune cost factors
@@ -214,8 +214,8 @@ vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) {
   // Steering angle (deltas)
   for (int i = delta_start; i < a_start; i++)
   {
-    vars_upperbound[i] = M_PI/4;
-    vars_lowerbound[i] = -M_PI/4;
+    vars_upperbound[i] = M_PI/8;
+    vars_lowerbound[i] = -M_PI/8;
   }
 
   // Acceleration

term-2/p5-model-predictive-control/src/main.cpp

@@ -102,6 +102,7 @@ int main() {
           */
 
           // Transform ptsx, ptsy to car coords
+          // TODO: implement in separate function
           for (int i = 0; i < ptsx.size(); i++) {
             double dtx = ptsx[i] - px;
             double dty = ptsy[i] - py;
@@ -128,16 +129,18 @@ int main() {
           // TODO: check derivation is correct
           double epsi = -atan(coeffs[1]);
 
-
           Eigen::VectorXd state(6);
           state << 0, 0, 0, v, cte, epsi;
           //state << px, py, psi, v, cte, epsi;
 
+          // predict the state 100ms into the future before you send it to the solver in order to compensate for the latency.
+
           // Solve using MPC
           auto result = mpc.Solve(state, coeffs);
 
-
-          double steer_value = result[0];
+          // ? Review said to convert to rad but it seems fine in the simulator
+          double steer_value = result[0]; // / deg2rad(25);
+          std::cout << "steer_value: " << steer_value << endl;
           double throttle_value = result[1];