Add ability to drive splines backwards
Change-Id: Ib34366ed155625fba56c4f72b9adce0615c6f12d
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();