Add spline management to base_autonomous_actor
We can now start and run splines. And it works!
Change-Id: Ic58cf8fb298538bd5446f00c2989f817561f9db1
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 7f778f7..73ebe10 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -46,8 +46,14 @@
// State of the spline execution.
bool is_executing;
- int32_t current_spline_handle;
+ // The handle of the goal spline. 0 means stop requested.
+ int32_t goal_spline_handle;
+ // Handle of the executing spline. -1 means none requested. If there was no
+ // spline executing when a spline finished optimizing, it will become the
+ // current spline even if we aren't ready to start yet.
int32_t current_spline_idx;
+ // Handle of the spline that is being optimized and staged.
+ int32_t planning_spline_idx;
// Expected position and velocity on the spline
float x;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 2621007..0b6026d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -41,6 +41,7 @@
plan_state_ = PlanState::kBuildingTrajectory;
const ::frc971::MultiSpline &multispline = goal_.spline;
future_spline_idx_ = multispline.spline_idx;
+ planning_spline_idx_ = future_spline_idx_;
auto x = multispline.spline_x;
auto y = multispline.spline_y;
::std::vector<Spline> splines = ::std::vector<Spline>();
@@ -187,8 +188,15 @@
}
status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
status->trajectory_logging.is_executing = !IsAtEnd();
- status->trajectory_logging.current_spline_handle = current_spline_handle_;
+ status->trajectory_logging.goal_spline_handle = current_spline_handle_;
status->trajectory_logging.current_spline_idx = current_spline_idx_;
+
+ int32_t planning_spline_idx = planning_spline_idx_;
+ if (current_spline_idx_ == planning_spline_idx) {
+ status->trajectory_logging.planning_spline_idx = 0;
+ } else {
+ status->trajectory_logging.planning_spline_idx = planning_spline_idx_;
+ }
}
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index c9ebbe4..c165693 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -99,6 +99,7 @@
::std::unique_ptr<Trajectory> past_trajectory_;
::std::unique_ptr<Trajectory> future_trajectory_;
int32_t future_spline_idx_ = 0; // Current spline being computed.
+ ::std::atomic<int32_t> planning_spline_idx_{-1};
// TODO(alex): pull this out of dt_config.
const ::Eigen::DiagonalMatrix<double, 5> Q =