Commit 19ec8265 by jsford

Fixed a bug in the vehicle dynamics.

parent 193d1711
......@@ -37,7 +37,7 @@ int main() {
// Define a reference trajectory that looks like stair steps.
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{10, 0, 0, 0, 1.0, 10.0});
refTraj.emplace_back(State{10, 1, 0, 0, 1.0, 11.0});
refTraj.emplace_back(State{20, 1, 0, 0, 1.0, 21.0});
......@@ -49,7 +49,7 @@ int main() {
// Choose an initial state that is slightly offset
// from the beginning of the reference trajectory.
State initialState (0, .5, 0, 0, 2.0, 0.0);
State initialState (0, 0.3, 0, 0, 2.0, 0.0);
// Choose zero initial control input.
Control initialControl {0, 0};
......@@ -74,7 +74,7 @@ int main() {
// Use the LQR trajectory tracker to smooth the reference trajectory.
Trajectory smoothTraj = trackLQR(initialState, initialControl, refTraj,
lqrParams, vehicleParams, 0.01);
lqrParams, vehicleParams, 0.05);
// Plot the smoothed trajectory.
plotStates(smoothTraj, "Tracked Trajectory");
......
......@@ -59,7 +59,7 @@ Trajectory trackLQR(const State& initialState,
const Trajectory& referenceTrajectory,
const LQRParams& lqrParams,
const VehicleParams& vehicleParams,
const double epsTime = 0.01);
const double epsTime = 0.05);
} // namespace track
#endif // TRACK_H_
......@@ -16,7 +16,7 @@ State dynamics(const State& state, const Control& control,
dState.y() = state.velocity() * std::sin(state.theta());
dState.theta() = std::tan(state.delta()) / params.wheelbase;
dState.delta() = control.deltaDot;
dState.velocity() += control.vDot;
dState.velocity() = control.vDot;
dState.time() = 1;
return dState;
......
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