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;
     }
   }
 

y2020/control_loops/superstructure/shooter/shooter.h

diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 7a3393b..2520df6 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -27,7 +27,9 @@
       flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
       const aos::monotonic_clock::time_point position_timestamp);
 
-  bool ready() { return ready_; }
+  bool ready() const { return ready_; }
+  bool finisher_ready() const { return finisher_ready_; }
+  bool accelerator_ready() const { return accelerator_ready_; }
 
   float finisher_goal() const { return finisher_.goal(); }
   float accelerator_goal() const { return accelerator_left_.goal(); }
@@ -43,6 +45,8 @@
   FlywheelController finisher_, accelerator_left_, accelerator_right_;
 
   bool UpToSpeed(const ShooterGoal *goal);
+  bool finisher_ready_ = false;
+  bool accelerator_ready_ = false;
   bool ready_ = false;
 
   int balls_shot_ = 0;