Fix current related issues
The Sign of the drive current was flipped and steer current was being multiplied by the motor inertia instead of Kt
Signed-off-by: justinT21 <jjturcot@gmail.com>
Change-Id: I4978aee05bf3d691b3f40d7b877d1dd2be97e588
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index 781943c..1326fd7 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -811,7 +811,7 @@
mul(add(neg(wb_), mul(add(rs_, rp_), sub(integer(1), div(rb1_, rp_)))),
mul(div(rw_, rb2_), neg(result.Fwx)));
RCP<const Basic> lhs =
- add(add(result.Ms, div(mul(Jsm_, result.Is), Gs_)), lhms);
+ add(add(result.Ms, div(mul(Kts_, result.Is), Gs_)), lhms);
RCP<const Basic> rhs = add(Jsm_, div(div(Js_, Gs_), Gs_));
RCP<const Basic> accel_steer_eqn = simplify(div(lhs, rhs));
@@ -826,9 +826,9 @@
mul(Gd1_, mul(Gd2_, alphamd)));
RCP<const Basic> ddplanitary_eqn = sub(mul(Gd3_, lhs), result.alphad);
- RCP<const Basic> drive_eqn = sub(
- add(mul(neg(Jdm_), div(alphamd, Gd_)), mul(Ktd_, div(result.Id, Gd_))),
- mul(neg(result.Fwx), rw_));
+ RCP<const Basic> drive_eqn = sub(add(mul(neg(Jdm_), div(alphamd, Gd_)),
+ mul(Ktd_, div(neg(result.Id), Gd_))),
+ mul(neg(result.Fwx), rw_));
VLOG(1) << "drive_eqn: " << drive_eqn->__str__();