Tune turret for real
Add in friction compensation and tune the controller a bit. Fix a poor
velocity estimate.
Change-Id: I153b659bf92d5f34f0e8f59fda34e3c693e41d6a
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 9a6c3cd..b9ba172 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -64,6 +64,17 @@
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
+ const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
+ GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
+
+ if (output != nullptr) {
+ // Friction is a pain and putting a really high burden on the integrator.
+ double velocity_sign = turret_status->velocity() * kTurretFrictionGain;
+ output_struct.turret_voltage +=
+ std::clamp(velocity_sign, -kTurretFrictionVoltageLimit,
+ kTurretFrictionVoltageLimit);
+ }
+
bool zeroed;
bool estopped;
@@ -74,9 +85,6 @@
const AbsoluteEncoderProfiledJointStatus *const intake_status =
GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
- const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
- GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
-
zeroed = hood_status->zeroed() && intake_status->zeroed() &&
turret_status->zeroed();
estopped = hood_status->estopped() || intake_status->estopped() ||