Moved AngularLinearToLeftRight to DrivetrainConfig
This makes it so the battlebot doesn't need SSDrivetrain. The
conversion functions also fit better with the drivetrain config
object.
Change-Id: Ife4e0ac0c0ebf2f15f3ce1012385579d9ca8eaa8
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 651976e..7bbba74 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -205,14 +205,14 @@
status->robot_speed = (kf_.X_hat(1, 0) + kf_.X_hat(3, 0)) / 2.0;
Eigen::Matrix<double, 2, 1> linear =
- dt_closedloop_.LeftRightToLinear(kf_.X_hat());
+ dt_config_.LeftRightToLinear(kf_.X_hat());
Eigen::Matrix<double, 2, 1> angular =
- dt_closedloop_.LeftRightToAngular(kf_.X_hat());
+ dt_config_.LeftRightToAngular(kf_.X_hat());
angular(0, 0) = integrated_kf_heading_;
Eigen::Matrix<double, 4, 1> gyro_left_right =
- dt_closedloop_.AngularLinearToLeftRight(linear, angular);
+ dt_config_.AngularLinearToLeftRight(linear, angular);
status->estimated_left_position = gyro_left_right(0, 0);
status->estimated_right_position = gyro_left_right(2, 0);
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index efb5f59..1fd633a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -55,6 +55,40 @@
double wheel_non_linearity;
double quickturn_wheel_multiplier;
+
+ // Converts the robot state to a linear distance position, velocity.
+ static Eigen::Matrix<double, 2, 1> LeftRightToLinear(
+ const Eigen::Matrix<double, 7, 1> &left_right) {
+ Eigen::Matrix<double, 2, 1> linear;
+ linear(0, 0) = (left_right(0, 0) + left_right(2, 0)) / 2.0;
+ linear(1, 0) = (left_right(1, 0) + left_right(3, 0)) / 2.0;
+ return linear;
+ }
+ // Converts the robot state to an anglular distance, velocity.
+ Eigen::Matrix<double, 2, 1> LeftRightToAngular(
+ const Eigen::Matrix<double, 7, 1> &left_right) const {
+ Eigen::Matrix<double, 2, 1> angular;
+ angular(0, 0) =
+ (left_right(2, 0) - left_right(0, 0)) / (this->robot_radius * 2.0);
+ angular(1, 0) =
+ (left_right(3, 0) - left_right(1, 0)) / (this->robot_radius * 2.0);
+ return angular;
+ }
+
+ // Converts the linear and angular position, velocity to the top 4 states of
+ // the robot state.
+ Eigen::Matrix<double, 4, 1> AngularLinearToLeftRight(
+ const Eigen::Matrix<double, 2, 1> &linear,
+ const Eigen::Matrix<double, 2, 1> &angular) const {
+ Eigen::Matrix<double, 2, 1> scaled_angle =
+ angular * this->robot_radius;
+ Eigen::Matrix<double, 4, 1> state;
+ state(0, 0) = linear(0, 0) - scaled_angle(0, 0);
+ state(1, 0) = linear(1, 0) - scaled_angle(1, 0);
+ state(2, 0) = linear(0, 0) + scaled_angle(0, 0);
+ state(3, 0) = linear(1, 0) + scaled_angle(1, 0);
+ return state;
+ }
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index e13c606..d3ea2f0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -353,27 +353,23 @@
// Tests that converting from a left, right position to a distance, angle
// coordinate system and back returns the same answer.
TEST_F(DrivetrainTest, LinearToAngularAndBack) {
- StateFeedbackLoop<7, 2, 3> kf(
- GetDrivetrainConfig().make_kf_drivetrain_loop());
- double kf_heading = 0;
- DrivetrainMotorsSS drivetrain_ss(GetDrivetrainConfig(), &kf, &kf_heading);
-
- const double width = GetDrivetrainConfig().robot_radius * 2.0;
+ const DrivetrainConfig dt_config = GetDrivetrainConfig();
+ const double width = dt_config.robot_radius * 2.0;
Eigen::Matrix<double, 7, 1> state;
state << 2, 3, 4, 5, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> linear = drivetrain_ss.LeftRightToLinear(state);
+ Eigen::Matrix<double, 2, 1> linear = dt_config.LeftRightToLinear(state);
EXPECT_NEAR(3.0, linear(0, 0), 1e-6);
EXPECT_NEAR(4.0, linear(1, 0), 1e-6);
- Eigen::Matrix<double, 2, 1> angular = drivetrain_ss.LeftRightToAngular(state);
+ Eigen::Matrix<double, 2, 1> angular = dt_config.LeftRightToAngular(state);
EXPECT_NEAR(2.0 / width, angular(0, 0), 1e-6);
EXPECT_NEAR(2.0 / width, angular(1, 0), 1e-6);
Eigen::Matrix<double, 4, 1> back_state =
- drivetrain_ss.AngularLinearToLeftRight(linear, angular);
+ dt_config.AngularLinearToLeftRight(linear, angular);
for (int i = 0; i < 4; ++i) {
EXPECT_NEAR(state(i, 0), back_state(i, 0), 1e-8);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index ff7ed6e..b7a68a9 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -154,38 +154,9 @@
angular_profile_.set_maximum_acceleration(goal.angular.max_acceleration);
}
-// (left + right) / 2 = linear
-// (right - left) / width = angular
-
-Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToLinear(
- const Eigen::Matrix<double, 7, 1> &left_right) const {
- Eigen::Matrix<double, 2, 1> linear;
- linear << (left_right(0, 0) + left_right(2, 0)) / 2.0,
- (left_right(1, 0) + left_right(3, 0)) / 2.0;
- return linear;
-}
-
-Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToAngular(
- const Eigen::Matrix<double, 7, 1> &left_right) const {
- Eigen::Matrix<double, 2, 1> angular;
- angular << (left_right(2, 0) - left_right(0, 0)) /
- (dt_config_.robot_radius * 2.0),
- (left_right(3, 0) - left_right(1, 0)) / (dt_config_.robot_radius * 2.0);
- return angular;
-}
-
-Eigen::Matrix<double, 4, 1> DrivetrainMotorsSS::AngularLinearToLeftRight(
- const Eigen::Matrix<double, 2, 1> &linear,
- const Eigen::Matrix<double, 2, 1> &angular) const {
- Eigen::Matrix<double, 2, 1> scaled_angle = angular * dt_config_.robot_radius;
- Eigen::Matrix<double, 4, 1> state;
- state << linear(0, 0) - scaled_angle(0, 0), linear(1, 0) - scaled_angle(1, 0),
- linear(0, 0) + scaled_angle(0, 0), linear(1, 0) + scaled_angle(1, 0);
- return state;
-}
-
void DrivetrainMotorsSS::Update(bool enable_control_loop) {
- Eigen::Matrix<double, 2, 1> wheel_heading = LeftRightToAngular(kf_->X_hat());
+ Eigen::Matrix<double, 2, 1> wheel_heading =
+ dt_config_.LeftRightToAngular(kf_->X_hat());
const double gyro_to_wheel_offset =
wheel_heading(0, 0) - *integrated_kf_heading_;
@@ -193,9 +164,9 @@
if (enable_control_loop) {
// Update profiles.
Eigen::Matrix<double, 2, 1> unprofiled_linear =
- LeftRightToLinear(unprofiled_goal_);
+ dt_config_.LeftRightToLinear(unprofiled_goal_);
Eigen::Matrix<double, 2, 1> unprofiled_angular =
- LeftRightToAngular(unprofiled_goal_);
+ dt_config_.LeftRightToAngular(unprofiled_goal_);
Eigen::Matrix<double, 2, 1> next_linear;
Eigen::Matrix<double, 2, 1> next_angular;
@@ -217,7 +188,7 @@
dt_config_.robot_radius;
kf_->mutable_next_R().block<4, 1>(0, 0) =
- AngularLinearToLeftRight(next_linear, next_angular);
+ dt_config_.AngularLinearToLeftRight(next_linear, next_angular);
kf_->mutable_next_R().block<3, 1>(4, 0) =
unprofiled_goal_.block<3, 1>(4, 0);
@@ -246,25 +217,26 @@
(kf_->U() - kf_->U_uncapped()).lpNorm<Eigen::Infinity>() > 1e-4) {
// kf_->R() is in wheel coordinates, while the profile is in absolute
// coordinates. Convert back...
- linear_profile_.MoveCurrentState(LeftRightToLinear(kf_->R()));
+ linear_profile_.MoveCurrentState(dt_config_.LeftRightToLinear(kf_->R()));
LOG(DEBUG, "Saturated while moving\n");
Eigen::Matrix<double, 2, 1> absolute_angular =
- LeftRightToAngular(kf_->R());
+ dt_config_.LeftRightToAngular(kf_->R());
absolute_angular(0, 0) -= gyro_to_wheel_offset;
angular_profile_.MoveCurrentState(absolute_angular);
}
} else {
- Eigen::Matrix<double, 2, 1> wheel_linear = LeftRightToLinear(kf_->X_hat());
+ Eigen::Matrix<double, 2, 1> wheel_linear =
+ dt_config_.LeftRightToLinear(kf_->X_hat());
Eigen::Matrix<double, 2, 1> next_angular = wheel_heading;
next_angular(0, 0) = *integrated_kf_heading_;
unprofiled_goal_.block<4, 1>(0, 0) =
- AngularLinearToLeftRight(wheel_linear, next_angular);
+ dt_config_.AngularLinearToLeftRight(wheel_linear, next_angular);
- auto current_linear = LeftRightToLinear(unprofiled_goal_);
- auto current_angular = LeftRightToAngular(unprofiled_goal_);
+ auto current_linear = dt_config_.LeftRightToLinear(unprofiled_goal_);
+ auto current_angular = dt_config_.LeftRightToAngular(unprofiled_goal_);
linear_profile_.MoveCurrentState(current_linear);
angular_profile_.MoveCurrentState(current_angular);
@@ -287,14 +259,14 @@
void DrivetrainMotorsSS::PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const {
Eigen::Matrix<double, 2, 1> profiled_linear =
- LeftRightToLinear(kf_->next_R());
+ dt_config_.LeftRightToLinear(kf_->next_R());
Eigen::Matrix<double, 2, 1> profiled_angular =
- LeftRightToAngular(kf_->next_R());
+ dt_config_.LeftRightToAngular(kf_->next_R());
profiled_angular(0, 0) -= last_gyro_to_wheel_offset_;
Eigen::Matrix<double, 4, 1> profiled_gyro_left_right =
- AngularLinearToLeftRight(profiled_linear, profiled_angular);
+ dt_config_.AngularLinearToLeftRight(profiled_linear, profiled_angular);
status->profiled_left_position_goal = profiled_gyro_left_right(0, 0);
status->profiled_left_velocity_goal = profiled_gyro_left_right(1, 0);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index c066c1b..ce12b19 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -38,19 +38,6 @@
void PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const;
- // Converts the robot state to a linear distance position, velocity.
- Eigen::Matrix<double, 2, 1> LeftRightToLinear(
- const Eigen::Matrix<double, 7, 1> &left_right) const;
- // Converts the robot state to an anglular distance, velocity.
- Eigen::Matrix<double, 2, 1> LeftRightToAngular(
- const Eigen::Matrix<double, 7, 1> &left_right) const;
-
- // Converts the linear and angular position, velocity to the top 4 states of
- // the robot state.
- Eigen::Matrix<double, 4, 1> AngularLinearToLeftRight(
- const Eigen::Matrix<double, 2, 1> &linear,
- const Eigen::Matrix<double, 2, 1> &angular) const;
-
private:
void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);