Move the trajectory computation into another thread.
Change-Id: I9dff7a20752e6cdfe05ec71d3435f3006ae45353
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index 18ed29e..a8e6b8c 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -208,7 +208,7 @@
// constraints.
struct MultiSpline {
// index of the spline. Zero indicates the spline should not be computed.
- uint32_t spline_idx;
+ int32_t spline_idx;
// Number of splines. The spline point arrays will be expected to have
// 6 + 5 * (n - 1) points in them. The endpoints are shared between
// neighboring splines.
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index f63f685..04bdaaf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -34,6 +34,29 @@
double right_velocity;
};
+// For logging information about the state of the trajectory planning.
+struct TrajectoryLogging {
+ // state of planning the trajectory.
+ // 0: not currently planning
+ // 1: received a multispline to plan
+ // 2: Built the spline and planning.
+ // 3: Finished the plan and ready to excecute.
+ int8_t planning_state;
+
+ // State of the spline execution.
+ bool is_executing;
+
+ int32_t current_spline_handle;
+ int32_t current_spline_idx;
+
+ // Expected position and velocity on the spline
+ float x;
+ float y;
+ float theta;
+ float left_velocity;
+ float right_velocity;
+};
+
queue_group DrivetrainQueue {
implements aos.control_loops.ControlLoop;
@@ -79,7 +102,7 @@
.frc971.MultiSpline spline;
// Which spline to follow.
- uint32_t spline_handle;
+ int32_t spline_handle;
};
message Position {
@@ -159,6 +182,7 @@
// Information about shifting logic and curent gear, for logging purposes
GearLogging gear_logging;
CIMLogging cim_logging;
+ TrajectoryLogging trajectory_logging;
};
queue Goal goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 363204c..707f136 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -74,10 +74,30 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
- void VerifyNearPosition(::Eigen::Matrix<double, 2, 1> expected) {
+ void VerifyNearSplineGoal() {
+ my_drivetrain_queue_.status.FetchLatest();
+ double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
+ double expected_y = my_drivetrain_queue_.status->trajectory_logging.y;
auto actual = drivetrain_motor_plant_.GetPosition();
- EXPECT_NEAR(actual(0), expected(0), 1e-2);
- EXPECT_NEAR(actual(1), expected(1), 1e-2);
+ EXPECT_NEAR(actual(0), expected_x, 3e-3);
+ EXPECT_NEAR(actual(1), expected_y, 3e-3);
+ }
+
+ void WaitForTrajectoryPlan() {
+ do {
+ // Run for fewer iterations while the worker thread computes.
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
+ RunIteration();
+ my_drivetrain_queue_.status.FetchLatest();
+ } while (my_drivetrain_queue_.status->trajectory_logging.planning_state !=
+ (int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
+ }
+
+ void WaitForTrajectoryExecution() {
+ do {
+ RunIteration();
+ my_drivetrain_queue_.status.FetchLatest();
+ } while (my_drivetrain_queue_.status->trajectory_logging.is_executing);
}
virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
@@ -358,14 +378,101 @@
goal.Send();
RunIteration();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
- my_drivetrain_queue_.goal.MakeMessage();
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
start_goal->controller_type = 2;
start_goal->spline_handle = 1;
start_goal.Send();
+ WaitForTrajectoryPlan();
RunForTime(chrono::milliseconds(2000));
- VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+ VerifyNearSplineGoal();
+}
+
+// Tests that simple spline with a single goal message.
+TEST_F(DrivetrainTest, SplineSingleGoal) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal->spline_handle = 1;
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearSplineGoal();
+}
+
+// Tests that a trajectory can be stopped in the middle.
+TEST_F(DrivetrainTest, SplineStop) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal->spline_handle = 1;
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ RunForTime(chrono::milliseconds(500));
+ my_drivetrain_queue_.status.FetchLatest();
+ const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
+ const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ stop_goal->controller_type = 2;
+ stop_goal->spline_handle = 0;
+ stop_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+
+ // The goal shouldn't change after being stopped.
+ my_drivetrain_queue_.status.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
+ EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
+}
+
+// Tests that a spline can't be restarted.
+TEST_F(DrivetrainTest, SplineRestart) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal->spline_handle = 1;
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ RunForTime(chrono::milliseconds(500));
+ my_drivetrain_queue_.status.FetchLatest();
+ const double goal_x = my_drivetrain_queue_.status->trajectory_logging.x;
+ const double goal_y = my_drivetrain_queue_.status->trajectory_logging.y;
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ stop_goal->controller_type = 2;
+ stop_goal->spline_handle = 0;
+ stop_goal.Send();
+ RunForTime(chrono::milliseconds(500));
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> restart_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ restart_goal->controller_type = 2;
+ restart_goal->spline_handle = 1;
+ restart_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+
+ // The goal shouldn't change after being stopped and restarted.
+ my_drivetrain_queue_.status.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.x, goal_x, 1e-9);
+ EXPECT_NEAR(my_drivetrain_queue_.status->trajectory_logging.y, goal_y, 1e-9);
}
// Tests that simple spline converges when it doesn't start where it thinks.
@@ -380,14 +487,15 @@
goal.Send();
RunIteration();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
- my_drivetrain_queue_.goal.MakeMessage();
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
start_goal->controller_type = 2;
start_goal->spline_handle = 1;
start_goal.Send();
+ WaitForTrajectoryPlan();
RunForTime(chrono::milliseconds(2000));
- VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+ VerifyNearSplineGoal();
}
// Tests that simple spline converges when it starts to the side of where it
@@ -403,14 +511,15 @@
goal.Send();
RunIteration();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
- my_drivetrain_queue_.goal.MakeMessage();
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
start_goal->controller_type = 2;
start_goal->spline_handle = 1;
start_goal.Send();
+ WaitForTrajectoryPlan();
RunForTime(chrono::milliseconds(2000));
- VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+ VerifyNearSplineGoal();
}
// Tests that a multispline converges on a goal.
@@ -420,19 +529,22 @@
goal->controller_type = 2;
goal->spline.spline_idx = 1;
goal->spline.spline_count = 2;
- goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ goal->spline.spline_x = {
+ {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ goal->spline.spline_y = {
+ {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
goal.Send();
RunIteration();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
- my_drivetrain_queue_.goal.MakeMessage();
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
start_goal->controller_type = 2;
start_goal->spline_handle = 1;
start_goal.Send();
+ WaitForTrajectoryPlan();
RunForTime(chrono::milliseconds(4000));
- VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+ VerifyNearSplineGoal();
}
// Tests that several splines converges on a goal.
@@ -443,20 +555,23 @@
goal->spline.spline_idx = 1;
goal->spline.spline_count = 1;
goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
goal.Send();
RunIteration();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
- my_drivetrain_queue_.goal.MakeMessage();
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
start_goal->controller_type = 2;
start_goal->spline_handle = 1;
start_goal.Send();
- RunForTime(chrono::milliseconds(2000));
- VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+ WaitForTrajectoryPlan();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_spline_goal =
- my_drivetrain_queue_.goal.MakeMessage();
+ WaitForTrajectoryExecution();
+
+ VerifyNearSplineGoal();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
second_spline_goal->controller_type = 2;
second_spline_goal->spline.spline_idx = 2;
second_spline_goal->spline.spline_count = 1;
@@ -465,14 +580,105 @@
second_spline_goal.Send();
RunIteration();
- ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_start_goal =
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
+ second_start_goal->controller_type = 2;
+ second_start_goal->spline_handle = 2;
+ second_start_goal.Send();
+ WaitForTrajectoryPlan();
+
+ RunForTime(chrono::milliseconds(2000));
+ VerifyNearSplineGoal();
+}
+// Tests that a second spline will run if the first is stopped.
+TEST_F(DrivetrainTest, SplineStopFirst) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+ goal->spline_handle = 1;
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ RunForTime(chrono::milliseconds(1000));
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> stop_goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ stop_goal->controller_type = 2;
+ stop_goal->spline_handle = 0;
+ stop_goal.Send();
+ RunForTime(chrono::milliseconds(500));
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
+ second_spline_goal->controller_type = 2;
+ second_spline_goal->spline.spline_idx = 2;
+ second_spline_goal->spline.spline_count = 1;
+ second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ second_spline_goal->spline_handle = 2;
+ second_spline_goal.Send();
+ WaitForTrajectoryPlan();
+
+ WaitForTrajectoryExecution();
+ VerifyNearSplineGoal();
+}
+
+// Tests that splines can excecute and plan at the same time.
+TEST_F(DrivetrainTest, ParallelSplines) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ second_spline_goal = my_drivetrain_queue_.goal.MakeMessage();
+ second_spline_goal->spline_handle = 1;
+ second_spline_goal->controller_type = 2;
+ second_spline_goal->spline.spline_idx = 2;
+ second_spline_goal->spline.spline_count = 1;
+ second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+ second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+ second_spline_goal.Send();
+ WaitForTrajectoryExecution();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ second_start_goal = my_drivetrain_queue_.goal.MakeMessage();
second_start_goal->controller_type = 2;
second_start_goal->spline_handle = 2;
second_start_goal.Send();
- RunForTime(chrono::milliseconds(2000));
- VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+ RunForTime(chrono::milliseconds(4000));
+ VerifyNearSplineGoal();
+}
+
+//Tests that a trajectory never told to execute will not replan.
+TEST_F(DrivetrainTest, OnlyPlanSpline) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ for (int i = 0; i < 100; ++i) {
+ RunIteration();
+ my_drivetrain_queue_.status.FetchLatest();
+ EXPECT_EQ(my_drivetrain_queue_.status->trajectory_logging.planning_state, 3);
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
+ }
+ VerifyNearSplineGoal();
}
::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 9aa1708..d8e2f9a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,38 +1,44 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
-#include <iostream>
-
#include "Eigen/Dense"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-const int kMaxSplineConstraints = 6;
-
namespace frc971 {
namespace control_loops {
namespace drivetrain {
SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
- : dt_config_(dt_config),
- current_state_(::Eigen::Matrix<double, 2, 1>::Zero()) {}
+ : dt_config_(dt_config), new_goal_(&mutex_) {
+ worker_thread_ = std::thread(&SplineDrivetrain::ComputeTrajectory, this);
+}
void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
- bool output_was_capped = ::std::abs((*U)(0, 0)) > 12.0 ||
- ::std::abs((*U)(1, 0)) > 12.0;
+ output_was_capped_ =
+ ::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
- if (output_was_capped) {
+ if (output_was_capped_) {
*U *= 12.0 / U->lpNorm<Eigen::Infinity>();
}
}
-// TODO(alex): put in another thread to avoid malloc in RT.
-void SplineDrivetrain::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- current_spline_handle_ = goal.spline_handle;
- const ::frc971::MultiSpline &multispline = goal.spline;
- if (multispline.spline_idx) {
- current_spline_idx_ = multispline.spline_idx;
+// TODO(alex): work on setting priority.
+void SplineDrivetrain::ComputeTrajectory() {
+ ::aos::MutexLocker locker(&mutex_);
+ while (run_) {
+ while (goal_.spline.spline_idx == future_spline_idx_) {
+ CHECK(!new_goal_.Wait());
+ if (!run_) {
+ return;
+ }
+ }
+ past_distance_spline_.reset();
+ past_trajectory_.reset();
+
+ plan_state_ = PlanState::kBuildingTrajectory;
+ const ::frc971::MultiSpline &multispline = goal_.spline;
+ future_spline_idx_ = multispline.spline_idx;
auto x = multispline.spline_x;
auto y = multispline.spline_y;
::std::vector<Spline> splines = ::std::vector<Spline>();
@@ -46,63 +52,100 @@
splines.emplace_back(Spline(points));
}
- distance_spline_ = ::std::unique_ptr<DistanceSpline>(
+ future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
new DistanceSpline(::std::move(splines)));
- current_trajectory_ = ::std::unique_ptr<Trajectory>(
- new Trajectory(distance_spline_.get(), dt_config_));
+ future_trajectory_ = ::std::unique_ptr<Trajectory>(
+ new Trajectory(future_distance_spline_.get(), dt_config_));
- for (int i = 0; i < kMaxSplineConstraints; ++i) {
+ for (size_t i = 0; i < multispline.constraints.size(); ++i) {
const ::frc971::Constraint &constraint = multispline.constraints[i];
switch (constraint.constraint_type) {
case 0:
break;
case 1:
- current_trajectory_->set_longitudal_acceleration(constraint.value);
+ future_trajectory_->set_longitudal_acceleration(constraint.value);
break;
case 2:
- current_trajectory_->set_lateral_acceleration(constraint.value);
+ future_trajectory_->set_lateral_acceleration(constraint.value);
break;
case 3:
- current_trajectory_->set_voltage_limit(constraint.value);
+ future_trajectory_->set_voltage_limit(constraint.value);
break;
case 4:
- current_trajectory_->LimitVelocity(constraint.start_distance,
- constraint.end_distance,
- constraint.value);
+ future_trajectory_->LimitVelocity(constraint.start_distance,
+ constraint.end_distance,
+ constraint.value);
break;
}
}
+ plan_state_ = PlanState::kPlanningTrajectory;
- current_trajectory_->Plan();
- current_xva_ = current_trajectory_->FFAcceleration(0);
- current_xva_(1) = 0.0;
- current_state_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+ future_trajectory_->Plan();
+ plan_state_ = PlanState::kPlannedTrajectory;
+ }
+}
+
+void SplineDrivetrain::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ current_spline_handle_ = goal.spline_handle;
+ // If told to stop, set the executing spline to an invalid index.
+ if (current_spline_handle_ == 0) {
+ current_spline_idx_ = -1;
+ }
+
+ ::aos::Mutex::State mutex_state = mutex_.TryLock();
+ if (mutex_state == ::aos::Mutex::State::kLocked) {
+ if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
+ goal_ = goal;
+ new_goal_.Broadcast();
+ }
+ if (future_trajectory_ &&
+ (!current_trajectory_ ||
+ current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) ||
+ current_spline_idx_ == -1)) {
+ // Move current data to other variables to be cleared by worker.
+ past_trajectory_ = std::move(current_trajectory_);
+ past_distance_spline_ = std::move(current_distance_spline_);
+
+ // Move the computed data to be executed.
+ current_trajectory_ = std::move(future_trajectory_);
+ current_distance_spline_ = std::move(future_distance_spline_);
+ current_spline_idx_ = future_spline_idx_;
+
+ // Reset internal state to a trajectory start position.
+ current_xva_ = current_trajectory_->FFAcceleration(0);
+ current_xva_(1) = 0.0;
+ }
+ mutex_.Unlock();
}
}
// TODO(alex): Hold position when done following the spline.
// TODO(Austin): Compensate for voltage error.
-void SplineDrivetrain::Update(bool enable,
- const ::Eigen::Matrix<double, 5, 1> &state) {
+void SplineDrivetrain::Update(bool enable, const ::Eigen::Matrix<double, 5, 1> &state) {
+ next_U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
enable_ = enable;
if (enable && current_trajectory_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- if (!IsAtEnd()) {
+ if (!IsAtEnd() &&
+ current_spline_handle_ == current_spline_idx_) {
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
U_ff = current_trajectory_->FFVoltage(current_xva_(0));
}
+
::Eigen::Matrix<double, 2, 5> K =
current_trajectory_->KForState(state, dt_config_.dt, Q, R);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+
+ ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2,1>(0,0);
+ next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &xv_state);
next_U_ = U_ff + U_fb;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
-
- next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, ¤t_state_);
}
}
@@ -111,9 +154,6 @@
if (!output) {
return;
}
- if (!current_trajectory_) {
- return;
- }
if (current_spline_handle_ == current_spline_idx_) {
if (!IsAtEnd()) {
output->left_voltage = next_U_(0);
@@ -121,6 +161,8 @@
current_xva_ = next_xva_;
}
}
+ output->left_voltage = next_U_(0);
+ output->right_voltage = next_U_(1);
}
void SplineDrivetrain::PopulateStatus(
@@ -129,6 +171,22 @@
status->uncapped_left_voltage = uncapped_U_(0);
status->uncapped_right_voltage = uncapped_U_(1);
status->robot_speed = current_xva_(1);
+ status->output_was_capped = output_was_capped_;
+ }
+
+ if (status) {
+ if (current_distance_spline_) {
+ ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+ status->trajectory_logging.x = goal_state(0);
+ status->trajectory_logging.y = goal_state(1);
+ status->trajectory_logging.theta = goal_state(2);
+ status->trajectory_logging.left_velocity = goal_state(3);
+ status->trajectory_logging.right_velocity = goal_state(4);
+ }
+ 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.current_spline_idx = current_spline_idx_;
}
}
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 =
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 45ae714..3b09e64 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -150,6 +150,16 @@
::std::copy(GetParam().control_pts_y.begin(),
GetParam().control_pts_y.end(), goal.spline.spline_y.begin());
spline_drivetrain_.SetGoal(goal);
+
+ // Let the spline drivetrain compute the spline.
+ ::frc971::control_loops::DrivetrainQueue::Status status;
+ do {
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
+ spline_drivetrain_.PopulateStatus(&status);
+ } while (status.trajectory_logging.planning_state !=
+ (int8_t)::frc971::control_loops::drivetrain::SplineDrivetrain::
+ PlanState::kPlannedTrajectory);
+ spline_drivetrain_.SetGoal(goal);
}
void TearDown() {