commit 0beb7dc3c7c70bec1486e0b29f740377b3875ccf
author milind-u <milind.upadhyay@gmail.com> Sat Oct 16 19:31:33 2021 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Oct 16 21:19:47 2021 -0700
tree b3380777e296fa5edef9dc1fac3289549f4b8d65
parent ae16c3bf020e4a3fa51ec5d557656e9b242563ba

List superstructure subsystems that aren't ready

Put this in the status for debugging why we aren't shooting yet

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I3ae07995a2b15b5db1c02863605e4079e1e2f571

y2020/control_loops/superstructure/shooter/shooter.cc

diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index e454aed..9f4edb2 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -24,21 +24,23 @@
                          accelerator::kBemf, accelerator::kResistance) {}
 
 bool Shooter::UpToSpeed(const ShooterGoal *goal) {
-  return (
-      std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
-          kVelocityToleranceFinisher &&
-      std::abs(goal->velocity_accelerator() -
-               accelerator_left_.avg_angular_velocity()) <
-          kVelocityToleranceAccelerator &&
-      std::abs(goal->velocity_accelerator() -
-               accelerator_right_.avg_angular_velocity()) <
-          kVelocityToleranceAccelerator &&
-      std::abs(goal->velocity_finisher() - finisher_.velocity()) <
-          kVelocityToleranceFinisher &&
-      std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
-          kVelocityToleranceAccelerator &&
-      std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
-          kVelocityToleranceAccelerator);
+  finisher_ready_ =
+      (std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
+           kVelocityToleranceFinisher &&
+       std::abs(goal->velocity_finisher() - finisher_.velocity()) <
+           kVelocityToleranceFinisher);
+  accelerator_ready_ =
+      (std::abs(goal->velocity_accelerator() -
+                accelerator_left_.avg_angular_velocity()) <
+           kVelocityToleranceAccelerator &&
+       std::abs(goal->velocity_accelerator() -
+                accelerator_right_.avg_angular_velocity()) <
+           kVelocityToleranceAccelerator &&
+       std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
+           kVelocityToleranceAccelerator &&
+       std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
+           kVelocityToleranceAccelerator);
+  return (finisher_ready_ && accelerator_ready_);
 }
 
 flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
@@ -78,6 +80,8 @@
       ready_ = true;
     } else {
       ready_ = false;
+      finisher_ready_ = false;
+      accelerator_ready_ = false;
     }
   }