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_;
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index 6c83721..0c0a564 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -120,6 +120,10 @@
'//aos/common/util:phased_loop',
'//aos/common/logging',
'//aos/common/actions:action_lib',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//y2016/control_loops/superstructure:superstructure_queue',
+ '//y2016/control_loops/drivetrain:drivetrain_base',
+ '//y2016/queues:profile_params',
],
)
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 60e0b7c..ad899be 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -26,6 +26,8 @@
bool VisionAlignActor::RunAction(
const actors::VisionAlignActionParams & /*params*/) {
+ const double robot_radius =
+ control_loops::drivetrain::GetDrivetrainConfig().robot_radius;
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
::aos::time::Time::InMS(5) / 2);
while (true) {
@@ -48,8 +50,7 @@
}
const double side_distance_change =
- ::y2016::vision::vision_status->horizontal_angle *
- control_loops::GetDrivetrainConfig().robot_radius;
+ ::y2016::vision::vision_status->horizontal_angle * robot_radius;
if (!::std::isfinite(side_distance_change)) {
continue;
}
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.cc b/y2016/control_loops/drivetrain/drivetrain_base.cc
index 0cdc447..2e64090 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_base.cc
@@ -12,6 +12,7 @@
namespace y2016 {
namespace control_loops {
+namespace drivetrain {
using ::frc971::constants::ShifterHallEffect;
@@ -40,5 +41,6 @@
return kDrivetrainConfig;
};
+} // namespace drivetrain
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.h b/y2016/control_loops/drivetrain/drivetrain_base.h
index 816ed21..c67031c 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.h
+++ b/y2016/control_loops/drivetrain/drivetrain_base.h
@@ -3,13 +3,14 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-
namespace y2016 {
namespace control_loops {
+namespace drivetrain {
-const DrivetrainConfig &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig &
+GetDrivetrainConfig();
+} // namespace drivetrain
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index 489a36e..deb7ae3 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -7,7 +7,8 @@
int main() {
::aos::Init();
- DrivetrainLoop drivetrain(::y2016::control_loops::GetDrivetrainConfig());
+ DrivetrainLoop drivetrain(
+ ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
drivetrain.Run();
::aos::Cleanup();
return 0;