Fixed drivetrain gyro compensated vs encoder errors.
Change-Id: Ib456bbc35652a26a55dace54a995e757050699b4
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_;