| commit | 5a543419bfa57c83b60e5dbc86a2b31c5496a7a9 | [log] [tgz] |
|---|---|---|
| author | James Kuszmaul <jabukuszmaul@gmail.com> | Sat Apr 13 15:49:42 2019 -0700 |
| committer | James Kuszmaul <jabukuszmaul@gmail.com> | Sun Apr 14 11:43:38 2019 -0700 |
| tree | 1f526225be85d168f60202066073360678268931 | |
| parent | c4eb1b2b2697881bdfb523c74478e8b89df4951f [diff] |
Add distance remaining to trajectory status Change-Id: I76b08464483f141aadae88ed53ca5814e1f75081
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q index 939ad75..c6af732 100644 --- a/frc971/control_loops/drivetrain/drivetrain.q +++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -63,6 +63,7 @@ float theta; float left_velocity; float right_velocity; + float distance_remaining; }; // For logging state of the line follower.
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc index 2eb434a..1f7e2b0 100644 --- a/frc971/control_loops/drivetrain/splinedrivetrain.cc +++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -218,6 +218,9 @@ (current_spline_idx_ != -1) && IsAtEnd(); status->trajectory_logging.goal_spline_handle = current_spline_handle_; status->trajectory_logging.current_spline_idx = current_spline_idx_; + status->trajectory_logging.distance_remaining = + current_trajectory_ ? current_trajectory_->length() - current_xva_.x() + : 0.0; int32_t planning_spline_idx = planning_spline_idx_; if (current_spline_idx_ == planning_spline_idx) {