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/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);