Re-enable polytopes for closed loop driving
I reproduced the original bug which resulted in this being disabled in a
test and fixed it already (I99a1299e538a395685c2e7e555dfccab48055349).
This commit fixes another issue observed in testing and re-enables the
polytopes.
Change-Id: Ic27e0ab8b2ea6dd480d7998c44530918443874db
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 1ed3eb7..193dbb2 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -27,9 +27,15 @@
private:
void CapU() override;
- const ::aos::controls::HPolytope<2> U_Poly_;
- Eigen::Matrix<double, 2, 2> T, T_inverse;
- bool output_was_capped_ = false;;
+ // 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_;
+
+ // multiplying by T converts [left_error, right_error] to
+ // [left_right_error_difference, total_distance_error].
+ Eigen::Matrix<double, 2, 2> T_, T_inverse_;
+
+ bool output_was_capped_ = false;
};
DrivetrainMotorsSS(const DrivetrainConfig &dt_config);