Add ability to drive splines backwards
Change-Id: Ib34366ed155625fba56c4f72b9adce0615c6f12d
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index ade396e..fc01edc 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -28,6 +28,7 @@
void BaseAutonomousActor::ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
max_drivetrain_voltage_ = 12.0;
+ goal_spline_handle_ = 0;
drivetrain_queue.goal.MakeWithBuilder()
.controller_type(0)
.highgear(true)
@@ -350,7 +351,7 @@
}
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
- const ::frc971::MultiSpline &spline) {
+ const ::frc971::MultiSpline &spline, SplineDirection direction) {
LOG(INFO, "Planning spline\n");
int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);
@@ -363,6 +364,8 @@
drivetrain_message->spline = spline;
drivetrain_message->spline.spline_idx = spline_handle;
drivetrain_message->spline_handle = goal_spline_handle_;
+ drivetrain_message->drive_spline_backwards =
+ direction == SplineDirection::kBackward;
LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
@@ -417,9 +420,17 @@
drivetrain_queue.status.FetchLatest();
LOG_STRUCT(INFO, "dts", *drivetrain_queue.status.get());
+ // We check that the spline we are waiting on is neither currently planning
+ // nor executing; note that we do *not* check the is_executing bit because
+ // immediately after calling Start we may still receive an old Status message
+ // that has not been updated. We check for planning so that the user can go
+ // straight from starting the planner to executing without a WaitForPlan in
+ // between.
if (drivetrain_queue.status.get() &&
- drivetrain_queue.status->trajectory_logging.current_spline_idx ==
- spline_handle_) {
+ (drivetrain_queue.status->trajectory_logging.current_spline_idx ==
+ spline_handle_ ||
+ drivetrain_queue.status->trajectory_logging.planning_spline_idx ==
+ spline_handle_)) {
return false;
}
return true;
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 7e3ef35..aeef38c 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -42,9 +42,16 @@
BaseAutonomousActor *base_autonomous_actor_;
};
+ // Represents the direction that we will drive along a spline.
+ enum class SplineDirection {
+ kForward,
+ kBackward,
+ };
+
// Starts planning the spline, and returns a handle to be used to manipulate
// it.
- SplineHandle PlanSpline(const ::frc971::MultiSpline &spline);
+ SplineHandle PlanSpline(const ::frc971::MultiSpline &spline,
+ SplineDirection direction);
void ResetDrivetrain();
void InitializeEncoders();
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 1480e4e..a1b96be 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -106,6 +106,7 @@
":spline",
":trajectory",
"//aos:init",
+ "//aos/util:math",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 73ebe10..09ecb4f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -130,6 +130,8 @@
// 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 05889e4..9d031b7 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -118,8 +118,8 @@
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_x, 1e-2);
- EXPECT_NEAR(actual(1), expected_y, 1e-2);
+ EXPECT_NEAR(actual(0), expected_x, 2e-2);
+ EXPECT_NEAR(actual(1), expected_y, 2e-2);
}
void WaitForTrajectoryPlan() {
@@ -435,6 +435,30 @@
VerifyNearSplineGoal();
}
+// Tests that we can drive a spline backwards.
+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.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();
+ RunIteration();
+
+ ::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));
+ VerifyNearSplineGoal();
+}
+
// Tests that simple spline with a single goal message.
TEST_F(DrivetrainTest, SplineSingleGoal) {
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
@@ -528,8 +552,8 @@
goal->controller_type = 2;
goal->spline.spline_idx = 1;
goal->spline.spline_count = 1;
- goal->spline.spline_x = {{0.5, 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_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.2, 0.0, 0.25 ,0.75, 1.0, 1.0}};
goal.Send();
RunIteration();
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index a37ca0e..18c8381 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -3,6 +3,7 @@
#include "Eigen/Dense"
#include "aos/init.h"
+#include "aos/util/math.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -99,6 +100,7 @@
::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();
@@ -143,7 +145,17 @@
::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_) {
+ ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
+ U_ff = -swapU;
+ goal_state(2, 0) += M_PI;
+ double left_goal = goal_state(3, 0);
+ double right_goal = goal_state(4, 0);
+ goal_state(3, 0) = -right_goal;
+ goal_state(4, 0) = -left_goal;
+ }
::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+ state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2,1>(0,0);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index d70cb6d..59175b6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -71,6 +71,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;
+ bool drive_spline_backwards_ = false;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;
@@ -105,8 +106,8 @@
// TODO(alex): pull this out of dt_config.
const ::Eigen::DiagonalMatrix<double, 5> Q =
(::Eigen::DiagonalMatrix<double, 5>().diagonal()
- << 1.0 / ::std::pow(0.07, 2),
- 1.0 / ::std::pow(0.07, 2), 1.0 / ::std::pow(0.2, 2),
+ << 1.0 / ::std::pow(0.12, 2),
+ 1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
1.0 / ::std::pow(1.5, 2), 1.0 / ::std::pow(1.5, 2))
.finished()
.asDiagonal();
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index b53e9d7..d30ee4b 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -410,7 +410,7 @@
// interest here are that we (a) stop adding disturbances at the very end of
// the trajectory, to allow us to actually converge to the goal, and (b)
// scale disturbances by the corruent velocity.
- if (GetParam().disturb && i % 50 == 0) {
+ if (GetParam().disturb && i % 75 == 0) {
// Scale the disturbance so that when we have near-zero velocity we don't
// have much disturbance.
double disturbance_scale = ::std::min(
@@ -582,7 +582,7 @@
.finished(),
/*noisify=*/false,
/*disturb=*/true,
- /*estimate_tolerance=*/2e-2,
+ /*estimate_tolerance=*/2.5e-2,
/*goal_tolerance=*/0.15,
}),
// Add noise and some initial error in addition: