Fix a bug in spline drivetrain.
Only allow a spline to be stopped if it has actually started.
Until now, a spline was 'started' as soon as it finished planning.
Change-Id: I083288703708f8d36c8bc7930bf465ff1e512b5c
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d4c765f..05889e4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -106,6 +106,13 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
+ void VerifyNearPosition(double x, double y) {
+ my_drivetrain_queue_.status.FetchLatest();
+ auto actual = drivetrain_motor_plant_.GetPosition();
+ EXPECT_NEAR(actual(0), x, 1e-2);
+ EXPECT_NEAR(actual(1), y, 1e-2);
+ }
+
void VerifyNearSplineGoal() {
my_drivetrain_queue_.status.FetchLatest();
double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
@@ -720,6 +727,29 @@
VerifyNearSplineGoal();
}
+//Tests that a trajectory can be executed after it is planned.
+TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
+ ::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();
+ RunForTime(chrono::milliseconds(2000));
+
+ ::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(1.0, 1.0);
+}
+
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0b6026d..a37ca0e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -93,7 +93,7 @@
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) {
+ if (current_spline_handle_ == 0 && has_started_execution_) {
current_spline_idx_ = -1;
}
@@ -119,6 +119,7 @@
// Reset internal state to a trajectory start position.
current_xva_ = current_trajectory_->FFAcceleration(0);
current_xva_(1) = 0.0;
+ has_started_execution_ = false;
}
mutex_.Unlock();
}
@@ -133,6 +134,7 @@
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
if (!IsAtEnd() &&
current_spline_handle_ == current_spline_idx_) {
+ has_started_execution_ = true;
// 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));
@@ -187,7 +189,7 @@
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.is_executing = !IsAtEnd() && has_started_execution_;
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 3e47f5a..d70cb6d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -70,6 +70,7 @@
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;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;