Make sundry improvements/bug fixes to auto-aiming
This includes:
-Adding some velocity and voltage limits to prevent excessive
velocity and acceleration.
-Logging more information in the status message.
-Fixing calculation of the desired turret velocity.
Change-Id: I83840bc1db9dda003be63e594d826fc97fa509e8
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 8947f85..0d740e9 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -40,7 +40,8 @@
if (drivetrain_status_fetcher_.Fetch()) {
aos::Alliance alliance = aos::Alliance::kInvalid;
- if (joystick_state_fetcher_.Fetch()) {
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() != nullptr) {
alliance = joystick_state_fetcher_->alliance();
}
const turret::Aimer::WrapMode mode =
@@ -120,7 +121,8 @@
if (output != nullptr) {
// Friction is a pain and putting a really high burden on the integrator.
- const double turret_velocity_sign = turret_status->velocity() * kTurretFrictionGain;
+ const double turret_velocity_sign =
+ turret_status->velocity() * kTurretFrictionGain;
output_struct.turret_voltage +=
std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
kTurretFrictionVoltageLimit);