Move the trajectory computation into another thread.
Change-Id: I9dff7a20752e6cdfe05ec71d3435f3006ae45353
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 2a79df6..c9ebbe4 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -1,8 +1,13 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
+#include <atomic>
+#include <thread>
+
#include "Eigen/Dense"
+#include "aos/condition.h"
+#include "aos/mutex/mutex.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -17,40 +22,83 @@
public:
SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
+ ~SplineDrivetrain() {
+ {
+ ::aos::MutexLocker locker(&mutex_);
+ run_ = false;
+ new_goal_.Signal();
+ }
+ worker_thread_.join();
+ }
+
void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
void SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output);
- // TODO(alex): What status do we need?
void PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const;
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
- Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
- return current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+ ::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
+ return current_trajectory_
+ ? current_trajectory_->GoalState(current_xva_(0),
+ current_xva_(1))
+ : ::Eigen::Matrix<double, 5, 1>::Zero();
}
bool IsAtEnd() const {
- return current_trajectory_->is_at_end(current_state_);
+ return current_trajectory_
+ ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) :
+ true;
}
+
+ enum class PlanState : int8_t {
+ kNoPlan = 0,
+ kBuildingTrajectory = 1,
+ kPlanningTrajectory = 2,
+ kPlannedTrajectory = 3,
+ };
+
private:
+ void ComputeTrajectory();
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
const DrivetrainConfig<double> dt_config_;
- uint32_t current_spline_handle_; // Current spline told to excecute.
- uint32_t current_spline_idx_; // Current excecuting spline.
- ::std::unique_ptr<DistanceSpline> distance_spline_;
- ::std::unique_ptr<Trajectory> current_trajectory_;
- ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
- ::Eigen::Matrix<double, 2, 1> current_state_;
- ::Eigen::Matrix<double, 2, 1> next_U_;
- ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+ int32_t current_spline_handle_ = 0; // Current spline told to excecute.
+ int32_t current_spline_idx_ = 0; // Current executing spline.
- bool enable_;
+ ::std::unique_ptr<DistanceSpline> current_distance_spline_;
+ ::std::unique_ptr<Trajectory> current_trajectory_;
+
+ // State required to compute the next iteration's output.
+ ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
+ ::Eigen::Matrix<double, 2, 1> next_U_;
+
+ // Information used for status message.
+ ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+ bool enable_ = false;
+ bool output_was_capped_ = false;
+
+ std::atomic<PlanState> plan_state_ = {PlanState::kNoPlan};
+
+ ::std::thread worker_thread_;
+ // mutex_ is held by the worker thread while it is doing work or by the main
+ // thread when it is sending work to the worker thread.
+ ::aos::Mutex mutex_;
+ // new_goal_ is used to signal to the worker thread that ther is work to do.
+ ::aos::Condition new_goal_;
+ // The following variables are guarded by mutex_.
+ bool run_ = true;
+ ::frc971::control_loops::DrivetrainQueue::Goal goal_;
+ ::std::unique_ptr<DistanceSpline> past_distance_spline_;
+ ::std::unique_ptr<DistanceSpline> future_distance_spline_;
+ ::std::unique_ptr<Trajectory> past_trajectory_;
+ ::std::unique_ptr<Trajectory> future_trajectory_;
+ int32_t future_spline_idx_ = 0; // Current spline being computed.
// TODO(alex): pull this out of dt_config.
const ::Eigen::DiagonalMatrix<double, 5> Q =