Commit 9f62a1db by Jordan Ford

Made the demo more interesting.

parent a82e8606
...@@ -22,15 +22,22 @@ void plotTrajectory(const Trajectory& traj, const std::string& title) { ...@@ -22,15 +22,22 @@ void plotTrajectory(const Trajectory& traj, const std::string& title) {
int main() { int main() {
Trajectory refTraj; Trajectory refTraj;
refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0}); refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0});
refTraj.emplace_back(State{40, 0, 0, 0, 1.0, 40.0}); refTraj.emplace_back(State{10, 0, 0, 0, 1.0, 10.0});
refTraj.emplace_back(State{10, 1, 0, 0, 1.0, 11.0});
State initialState{0, .1, 0, 0, 2.0, 0.0}; refTraj.emplace_back(State{20, 1, 0, 0, 1.0, 21.0});
refTraj.emplace_back(State{20, 2, 0, 0, 1.0, 22.0});
refTraj.emplace_back(State{30, 2, 0, 0, 1.0, 32.0});
refTraj.emplace_back(State{30, 3, 0, 0, 1.0, 33.0});
refTraj.emplace_back(State{40, 3, 0, 0, 1.0, 43.0});
refTraj.emplace_back(State{40, 4, 0, 0, 1.0, 44.0});
State initialState{0, .5, 0, 0, 2.0, 0.0};
Control initialControl{0, 0}; Control initialControl{0, 0};
VehicleParams vehicleParams{3.0}; // vehicle wheelbase length VehicleParams vehicleParams{2.0}; // vehicle wheelbase length
LQRParams lqrParams; LQRParams lqrParams;
lqrParams.qLat << 1000, 0, 0, // d lqrParams.qLat << 10000, 0, 0, // d
0, 1, 0, // theta* - theta 0, 1, 0, // theta* - theta
0, 0, 1000; // delta 0, 0, 1000; // delta
...@@ -45,6 +52,11 @@ int main() { ...@@ -45,6 +52,11 @@ int main() {
lqrParams, vehicleParams, 0.01); lqrParams, vehicleParams, 0.01);
plotTrajectory(smoothTraj, "Tracked Trajectory"); plotTrajectory(smoothTraj, "Tracked Trajectory");
plotTrajectory(refTraj, "Tracked Trajectory");
plt::xlim(0, 40);
plt::ylim(-10, 10);
plt::xlabel("X");
plt::ylabel("Y");
plt::show(); plt::show();
return 0; return 0;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment