Keep a count of the total number of balls shot
When a ball is in the flywheel, the friction causes
the flywheel velocity to slow down. It speeds up again when the
ball is shot. We can use this to determine when balls are shot.
For more details on this thought process, see
https://docs.google.com/document/d/1XHSnfcRiM3RihTK9LcC3zZn9kPMiARX02v4XhrQpidE/edit?usp=sharing
Change-Id: I2a0378c94cbc364f18c0af0df1706f046daf530b
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 900d0f1..9580ef8 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -1,6 +1,7 @@
#include "y2020/control_loops/superstructure/shooter/shooter.h"
#include <chrono>
+#include <cmath>
#include "aos/logging/logging.h"
#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
@@ -47,6 +48,8 @@
const ShooterGoal *goal, const ShooterPosition *position,
flatbuffers::FlatBufferBuilder *fbb, OutputT *output,
const aos::monotonic_clock::time_point position_timestamp) {
+ const double last_finisher_velocity = finisher_.velocity();
+
// Update position, output, and status for our two shooter sides.
finisher_.set_position(position->theta_finisher(), position_timestamp);
accelerator_left_.set_position(position->theta_accelerator_left(),
@@ -56,6 +59,12 @@
// Update goal.
if (goal) {
+ if (std::abs(goal->velocity_finisher() - finisher_goal()) >=
+ kVelocityTolerance) {
+ finisher_goal_changed_ = true;
+ last_finisher_velocity_max_ = 0.0;
+ }
+
finisher_.set_goal(goal->velocity_finisher());
accelerator_left_.set_goal(goal->velocity_accelerator());
accelerator_right_.set_goal(goal->velocity_accelerator());
@@ -88,6 +97,41 @@
status_builder.add_accelerator_right(accelerator_right_status_offset);
status_builder.add_ready(ready());
+ if (finisher_goal_changed_) {
+ // If we have caught up to the new goal, we can start detecting if a ball
+ // was shot.
+ finisher_goal_changed_ =
+ (std::abs(finisher_.velocity() - finisher_goal()) > kVelocityTolerance);
+ }
+
+ if (!finisher_goal_changed_) {
+ const bool finisher_was_accelerating = finisher_accelerating_;
+ finisher_accelerating_ = (finisher_.velocity() > last_finisher_velocity);
+ if (finisher_was_accelerating && !finisher_accelerating_) {
+ last_finisher_velocity_max_ = std::min(
+ last_finisher_velocity, static_cast<double>(finisher_goal()));
+ }
+
+ const double finisher_velocity_dip =
+ last_finisher_velocity_max_ - finisher_.velocity();
+
+ if (finisher_velocity_dip < kVelocityTolerance && ball_in_finisher_) {
+ // If we detected a ball in the flywheel and now the angular velocity has
+ // come back up close to the last local maximum or is greater than it, the
+ // ball has been shot.
+ balls_shot_++;
+ ball_in_finisher_ = false;
+ } else if (!ball_in_finisher_ && (finisher_goal() > kVelocityTolerance)) {
+ // There is probably a ball in the flywheel if the angular
+ // velocity is atleast kMinVelocityErrorWithBall less than the last local
+ // maximum.
+ ball_in_finisher_ =
+ (finisher_velocity_dip >= kMinFinisherVelocityDipWithBall);
+ }
+ }
+
+ status_builder.add_balls_shot(balls_shot_);
+
if (output) {
output->finisher_voltage = finisher_.voltage();
output->accelerator_left_voltage = accelerator_left_.voltage();
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index 539ecc9..f41b882 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -30,11 +30,26 @@
float accelerator_goal() const { return accelerator_left_.goal(); }
private:
+ // Minumum difference between the last local maximum finisher angular velocity
+ // and the current finisher angular velocity when we have a ball in the
+ // flywheel, in radians/s. This arises because the flywheel slows down when
+ // there is a ball in it. We can use this to determine when a ball is in the
+ // flywheel and when it gets shot.
+ static constexpr double kMinFinisherVelocityDipWithBall = 5.0;
+
FlywheelController finisher_, accelerator_left_, accelerator_right_;
bool UpToSpeed(const ShooterGoal *goal);
bool ready_ = false;
+ int balls_shot_ = 0;
+ bool finisher_goal_changed_ = false;
+ bool ball_in_finisher_ = false;
+ // Last local maximum in the finisher angular velocity
+ double last_finisher_velocity_max_ = 0.0;
+ // True if the finisher's average acceleration over the last dt is positive
+ bool finisher_accelerating_ = false;
+
DISALLOW_COPY_AND_ASSIGN(Shooter);
};