Allow cancelling un-executed splines
Previously we couldn't cancel a spline without starting execution of it.
Also, allow specifying driving backwards on a per-spline basis.
Change-Id: I7a1fc54a49bfdcfe8c8614bde18d2976ba3a7868
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.q
index a8e6b8c..2359bcd 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.q
@@ -216,5 +216,8 @@
float[36] spline_x;
float[36] spline_y;
+ // Whether to follow the spline driving forwards or backwards.
+ bool drive_spline_backwards;
+
Constraint[6] constraints;
};
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 09ecb4f..939ad75 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -45,6 +45,8 @@
// State of the spline execution.
bool is_executing;
+ // Whether we have finished the spline specified by current_spline_idx.
+ bool is_executed;
// The handle of the goal spline. 0 means stop requested.
int32_t goal_spline_handle;
@@ -130,8 +132,6 @@
// Which spline to follow.
int32_t spline_handle;
- // Whether to follow the spline driving forwards or backwards.
- bool drive_spline_backwards;
};
message Position {
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 9d031b7..81506f2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -136,7 +136,7 @@
do {
RunIteration();
my_drivetrain_queue_.status.FetchLatest();
- } while (my_drivetrain_queue_.status->trajectory_logging.is_executing);
+ } while (!my_drivetrain_queue_.status->trajectory_logging.is_executed);
}
virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
@@ -439,10 +439,10 @@
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
my_drivetrain_queue_.goal.MakeMessage();
- goal->drive_spline_backwards = true;
goal->controller_type = 2;
goal->spline.spline_idx = 1;
goal->spline.spline_count = 1;
+ goal->spline.drive_spline_backwards = true;
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();
@@ -455,8 +455,26 @@
start_goal.Send();
WaitForTrajectoryPlan();
- RunForTime(chrono::milliseconds(2000));
+ // Check that we are right on the spline at the start (otherwise the feedback
+ // will tend to correct for going the wrong direction).
+ for (int ii = 0; ii < 10; ++ii) {
+ RunForTime(chrono::milliseconds(100));
+ VerifyNearSplineGoal();
+ }
+
+ WaitForTrajectoryExecution();
+
VerifyNearSplineGoal();
+ // Check that we are pointed the right direction:
+ my_drivetrain_queue_.status.FetchLatest();
+ auto actual = drivetrain_motor_plant_.state();
+ const double expected_theta =
+ my_drivetrain_queue_.status->trajectory_logging.theta;
+ // As a sanity check, compare both against absolute angle and the spline's
+ // goal angle.
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta),
+ 2e-2);
}
// Tests that simple spline with a single goal message.
@@ -697,6 +715,46 @@
VerifyNearSplineGoal();
}
+// Tests that we can run a second spline after having planned but never executed
+// the first spline.
+TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
+ ::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}};
+ // Don't start running the splane.
+ goal->spline_handle = 0;
+ goal.Send();
+ WaitForTrajectoryPlan();
+
+ RunForTime(chrono::milliseconds(1000));
+
+ // Plan another spline, but don't start it yet:
+ ::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 = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
+ second_spline_goal->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
+ second_spline_goal->spline_handle = 0;
+ second_spline_goal.Send();
+ WaitForTrajectoryPlan();
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ execute_goal = my_drivetrain_queue_.goal.MakeMessage();
+ execute_goal->controller_type = 2;
+ execute_goal->spline_handle = 2;
+ execute_goal.Send();
+
+ WaitForTrajectoryExecution();
+ VerifyNearSplineGoal();
+ VerifyNearPosition(1.0, 2.0);
+}
+
// Tests that splines can excecute and plan at the same time.
TEST_F(DrivetrainTest, ParallelSplines) {
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 18c8381..2eb434a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -56,6 +56,8 @@
splines.emplace_back(Spline(points));
}
+ future_drive_spline_backwards_ = goal_.spline.drive_spline_backwards;
+
future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
new DistanceSpline(::std::move(splines)));
@@ -93,18 +95,26 @@
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 && has_started_execution_) {
+ // If told to stop, set the executing spline to an invalid index and clear out
+ // its plan:
+ if (current_spline_handle_ == 0 &&
+ current_spline_idx_ != goal.spline.spline_idx) {
current_spline_idx_ = -1;
}
::aos::Mutex::State mutex_state = mutex_.TryLock();
if (mutex_state == ::aos::Mutex::State::kLocked) {
- drive_spline_backwards_ = goal_.drive_spline_backwards;
if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
goal_ = goal;
new_goal_.Broadcast();
+ if (current_spline_handle_ != current_spline_idx_) {
+ // If we aren't going to actively execute the current spline, evict it's
+ // plan.
+ past_trajectory_ = std::move(current_trajectory_);
+ past_distance_spline_ = std::move(current_distance_spline_);
+ }
}
+ // If you never started executing the previous spline, you're screwed.
if (future_trajectory_ &&
(!current_trajectory_ ||
current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) ||
@@ -116,6 +126,7 @@
// Move the computed data to be executed.
current_trajectory_ = std::move(future_trajectory_);
current_distance_spline_ = std::move(future_distance_spline_);
+ current_drive_spline_backwards_ = future_drive_spline_backwards_;
current_spline_idx_ = future_spline_idx_;
// Reset internal state to a trajectory start position.
@@ -145,7 +156,7 @@
::Eigen::Matrix<double, 2, 5> K =
current_trajectory_->KForState(state, dt_config_.dt, Q, R);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- if (drive_spline_backwards_) {
+ if (current_drive_spline_backwards_) {
::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
U_ff = -swapU;
goal_state(2, 0) += M_PI;
@@ -196,12 +207,15 @@
::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.theta = ::aos::math::NormalizeAngle(
+ goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0));
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() && has_started_execution_;
+ status->trajectory_logging.is_executed =
+ (current_spline_idx_ != -1) && IsAtEnd();
status->trajectory_logging.goal_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 59175b6..98c4e74 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -71,10 +71,10 @@
int32_t current_spline_handle_ = 0; // Current spline told to excecute.
int32_t current_spline_idx_ = 0; // Current executing spline.
bool has_started_execution_ = false;
- bool drive_spline_backwards_ = false;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;
+ bool current_drive_spline_backwards_ = false;
// State required to compute the next iteration's output.
::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
@@ -100,6 +100,7 @@
::std::unique_ptr<DistanceSpline> future_distance_spline_;
::std::unique_ptr<Trajectory> past_trajectory_;
::std::unique_ptr<Trajectory> future_trajectory_;
+ bool future_drive_spline_backwards_ = false;
int32_t future_spline_idx_ = 0; // Current spline being computed.
::std::atomic<int32_t> planning_spline_idx_{-1};