Fixed drivetrain gyro compensated vs encoder errors.
Change-Id: Ib456bbc35652a26a55dace54a995e757050699b4
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index c1db075..235738d 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -148,7 +148,7 @@
// (right - left) / width = angular
Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToLinear(
- const Eigen::Matrix<double, 7, 1> &left_right) {
+ 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;
@@ -156,7 +156,7 @@
}
Eigen::Matrix<double, 2, 1> DrivetrainMotorsSS::LeftRightToAngular(
- const Eigen::Matrix<double, 7, 1> &left_right) {
+ 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),
@@ -166,7 +166,7 @@
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> &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),
@@ -175,15 +175,17 @@
}
void DrivetrainMotorsSS::Update(bool enable_control_loop) {
+ Eigen::Matrix<double, 2, 1> wheel_heading = LeftRightToAngular(kf_->X_hat());
+
+ const double gyro_to_wheel_offset =
+ wheel_heading(0, 0) - *integrated_kf_heading_;
+
if (enable_control_loop) {
// Update profiles.
Eigen::Matrix<double, 2, 1> unprofiled_linear =
- LeftRightToLinear(unprofiled_goal_);
+ LeftRightToLinear(unprofiled_goal_);
Eigen::Matrix<double, 2, 1> unprofiled_angular =
- LeftRightToAngular(unprofiled_goal_);
-
- Eigen::Matrix<double, 2, 1> wheel_heading =
- LeftRightToAngular(kf_->X_hat());
+ LeftRightToAngular(unprofiled_goal_);
Eigen::Matrix<double, 2, 1> next_linear;
Eigen::Matrix<double, 2, 1> next_angular;
@@ -193,13 +195,16 @@
unprofiled_linear(1, 0));
next_angular = angular_profile_.Update(unprofiled_angular(0, 0),
unprofiled_angular(1, 0));
-
} else {
next_angular = unprofiled_angular;
next_linear = unprofiled_linear;
}
- next_angular(0, 0) += wheel_heading(0, 0) - *integrated_kf_heading_;
+ const double wheel_compensation_offset =
+ gyro_to_wheel_offset * dt_config_.robot_radius;
+ const double scaled_angle_delta =
+ (gyro_to_wheel_offset - last_gyro_to_wheel_offset_) *
+ dt_config_.robot_radius;
kf_->mutable_next_R().block<4, 1>(0, 0) =
AngularLinearToLeftRight(next_linear, next_angular);
@@ -207,8 +212,14 @@
kf_->mutable_next_R().block<3, 1>(4, 0) =
unprofiled_goal_.block<3, 1>(4, 0);
+ kf_->mutable_next_R(0, 0) -= wheel_compensation_offset;
+ kf_->mutable_next_R(2, 0) += wheel_compensation_offset;
+
if (!use_profile_) {
kf_->mutable_R() = kf_->next_R();
+ } else {
+ kf_->mutable_R(0, 0) -= scaled_angle_delta;
+ kf_->mutable_R(2, 0) += scaled_angle_delta;
}
// Run the controller.
@@ -223,21 +234,34 @@
// Now, move the profile if things didn't go perfectly.
if (use_profile_ &&
(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()));
- angular_profile_.MoveCurrentState(LeftRightToAngular(kf_->R()));
+
+ LOG(DEBUG, "Saturated while moving\n");
+
+ Eigen::Matrix<double, 2, 1> absolute_angular =
+ LeftRightToAngular(kf_->R());
+ absolute_angular(0, 0) -= gyro_to_wheel_offset;
+ angular_profile_.MoveCurrentState(absolute_angular);
}
} else {
- unprofiled_goal_(0, 0) = kf_->X_hat()(0, 0);
- unprofiled_goal_(1, 0) = 0.0;
- unprofiled_goal_(2, 0) = kf_->X_hat()(2, 0);
- unprofiled_goal_(3, 0) = 0.0;
+ Eigen::Matrix<double, 2, 1> wheel_linear = LeftRightToLinear(kf_->X_hat());
+ Eigen::Matrix<double, 2, 1> next_angular = wheel_heading;
+ next_angular(0, 0) = *integrated_kf_heading_;
- linear_profile_.MoveCurrentState(LeftRightToLinear(unprofiled_goal_));
- angular_profile_.MoveCurrentState(LeftRightToAngular(unprofiled_goal_));
+ unprofiled_goal_.block<4, 1>(0, 0) =
+ AngularLinearToLeftRight(wheel_linear, next_angular);
- kf_->mutable_next_R() = unprofiled_goal_;
- kf_->mutable_R() = unprofiled_goal_;
+ auto current_linear = LeftRightToLinear(unprofiled_goal_);
+ auto current_angular = LeftRightToAngular(unprofiled_goal_);
+ linear_profile_.MoveCurrentState(current_linear);
+ angular_profile_.MoveCurrentState(current_angular);
+
+ kf_->mutable_next_R().block<4, 1>(0, 0) = kf_->X_hat().block<4, 1>(0, 0);
+ kf_->mutable_R().block<4, 1>(0, 0) = kf_->X_hat().block<4, 1>(0, 0);
}
+ last_gyro_to_wheel_offset_ = gyro_to_wheel_offset;
}
void DrivetrainMotorsSS::SetOutput(
@@ -252,10 +276,20 @@
void DrivetrainMotorsSS::PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const {
- status->profiled_left_position_goal = kf_->next_R(0, 0);
- status->profiled_left_velocity_goal = kf_->next_R(1, 0);
- status->profiled_right_position_goal = kf_->next_R(2, 0);
- status->profiled_right_velocity_goal = kf_->next_R(3, 0);
+ Eigen::Matrix<double, 2, 1> profiled_linear =
+ LeftRightToLinear(kf_->next_R());
+ Eigen::Matrix<double, 2, 1> profiled_angular =
+ 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);
+
+ status->profiled_left_position_goal = profiled_gyro_left_right(0, 0);
+ status->profiled_left_velocity_goal = profiled_gyro_left_right(1, 0);
+ status->profiled_right_position_goal = profiled_gyro_left_right(2, 0);
+ status->profiled_right_velocity_goal = profiled_gyro_left_right(3, 0);
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 419b401..15a4823 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -40,16 +40,16 @@
// 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 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 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 Eigen::Matrix<double, 2, 1> &angular) const;
private:
void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
@@ -59,6 +59,8 @@
StateFeedbackLoop<7, 2, 3> *kf_;
Eigen::Matrix<double, 7, 1> unprofiled_goal_;
+ double last_gyro_to_wheel_offset_ = 0;
+
// Reprsents +/- full power on each motor in U-space, aka the square from
// (-12, -12) to (12, 12).
const ::aos::controls::HPolytope<2> U_poly_;