added support for both drivetrains with the same code
Previously, we were just switching back and forth manually, which
doesn't work very well. The python code for (both) drivetrain loops now
generates constants for both robots and frc971/constants knows which one
to give the actual drivetrain control loop code.
Various other cleanups to things that were so hard to read it made it
difficult to figure out how to do this too.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 815f05e..2b16c0b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -11,8 +11,6 @@
#include "aos/controls/polytope.h"
#include "aos/common/commonmath.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain_motor_plant.h"
-#include "frc971/control_loops/drivetrain/polydrivetrain_motor_plant.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -76,8 +74,9 @@
class DrivetrainMotorsSS {
public:
- DrivetrainMotorsSS ()
- : loop_(new StateFeedbackLoop<4, 2, 2>(MakeDrivetrainLoop())) {
+ DrivetrainMotorsSS()
+ : loop_(new StateFeedbackLoop<4, 2, 2>(
+ constants::GetValues().make_drivetrain_loop())) {
_offset = 0;
_integral_offset = 0;
_left_goal = 0.0;
@@ -181,7 +180,8 @@
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]]*/).finished()),
- loop_(new StateFeedbackLoop<2, 2, 2>(MakeVDrivetrainLoop())),
+ loop_(new StateFeedbackLoop<2, 2, 2>(
+ constants::GetValues().make_v_drivetrain_loop())),
left_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
right_cim_(new StateFeedbackLoop<1, 1, 1>(MakeCIMLoop())),
ttrust_(1.1),