Compensate steer coupling torque from velocity cost
We don't want to penalize using steer current to stay straight.
Compensate it out of the cost function.
Change-Id: I649ddb1b4c94b6746ea661252964b8f85633fa5c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index e712a39..19b4f12 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -574,6 +574,16 @@
result_py.emplace_back(absl::Substitute(
" [1.0, casadi.fabs(x) * $0.0]))) / $0.0)", kLogGain));
result_py.emplace_back("");
+ result_py.emplace_back("# Is = STEER_CURRENT_COUPLING_FACTOR * Id");
+ result_py.emplace_back(absl::Substitute(
+ "STEER_CURRENT_COUPLING_FACTOR = $0",
+ ccode(*(neg(
+ mul(div(Gs_, Kts_),
+ mul(div(Ktd_, mul(Gd_, rw_)),
+ neg(mul(add(neg(wb_), mul(add(rs_, rp_),
+ sub(integer(1), div(rb1_, rp_)))),
+ div(rw_, rb2_))))))))));
+ result_py.emplace_back("");
result_py.emplace_back("# Returns the derivative of our state vector");
result_py.emplace_back("# [thetas0, thetad0, omegas0, omegad0,");