Fix bug in physics and a few in physics_debug
It turns out we didn't rotate the mounting location when crossing it with force to get torque. We also didn't use calculate current using the PID when calculating the xdot after integrating it. I also added a few more plots that may be useful for debugging in the future.
Signed-off-by: justinT21 <jjturcot@gmail.com>
Change-Id: I17357833debdfd34977e9ef3f8b11812a835ddce
diff --git a/frc971/control_loops/swerve/generate_physics.cc b/frc971/control_loops/swerve/generate_physics.cc
index e796ec6..748e3cb 100644
--- a/frc971/control_loops/swerve/generate_physics.cc
+++ b/frc971/control_loops/swerve/generate_physics.cc
@@ -1015,14 +1015,18 @@
mul_dense_dense(R(add(theta_, result.thetas)),
DenseMatrix(2, 1, {result.full.Fwx, result.Fwy}),
result.full.F);
- result.full.torque = force_cross(result.mounting_location, result.full.F);
+
+ DenseMatrix rotated_mounting_location = DenseMatrix(2, 1);
+ mul_dense_dense(R(theta_), result.mounting_location,
+ rotated_mounting_location);
+ result.full.torque = force_cross(rotated_mounting_location, result.full.F);
result.direct.F = DenseMatrix(2, 1);
mul_dense_dense(R(add(theta_, result.thetas)),
DenseMatrix(2, 1, {result.direct.Fwx, result.Fwy}),
result.direct.F);
result.direct.torque =
- force_cross(result.mounting_location, result.direct.F);
+ force_cross(rotated_mounting_location, result.direct.F);
VLOG(1);
VLOG(1) << "full torque = " << result.full.torque->__str__();