- 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