Relax the shooter ready velocity tolerances
We were chattering between ready and not ready a *lot*. Open up the
tolerances a bit so we are faster to get the shot off.
We should consider hysterisis as well at some point.
Change-Id: I952cebc4810ab4aec5e782e1dd8e0f076ac1a123
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
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 9580ef8..e454aed 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -12,10 +12,6 @@
namespace superstructure {
namespace shooter {
-namespace {
-const double kVelocityTolerance = 2.0;
-} // namespace
-
Shooter::Shooter()
: finisher_(
finisher::MakeIntegralFinisherLoop(), finisher::kBemf,
@@ -30,18 +26,19 @@
bool Shooter::UpToSpeed(const ShooterGoal *goal) {
return (
std::abs(goal->velocity_finisher() - finisher_.avg_angular_velocity()) <
- kVelocityTolerance &&
+ kVelocityToleranceFinisher &&
std::abs(goal->velocity_accelerator() -
- accelerator_left_.avg_angular_velocity()) < kVelocityTolerance &&
+ accelerator_left_.avg_angular_velocity()) <
+ kVelocityToleranceAccelerator &&
std::abs(goal->velocity_accelerator() -
accelerator_right_.avg_angular_velocity()) <
- kVelocityTolerance &&
+ kVelocityToleranceAccelerator &&
std::abs(goal->velocity_finisher() - finisher_.velocity()) <
- kVelocityTolerance &&
+ kVelocityToleranceFinisher &&
std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
- kVelocityTolerance &&
+ kVelocityToleranceAccelerator &&
std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
- kVelocityTolerance);
+ kVelocityToleranceAccelerator);
}
flatbuffers::Offset<ShooterStatus> Shooter::RunIteration(
@@ -60,7 +57,7 @@
// Update goal.
if (goal) {
if (std::abs(goal->velocity_finisher() - finisher_goal()) >=
- kVelocityTolerance) {
+ kVelocityToleranceFinisher) {
finisher_goal_changed_ = true;
last_finisher_velocity_max_ = 0.0;
}
@@ -75,8 +72,9 @@
accelerator_right_.Update(output == nullptr);
if (goal) {
- if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
- goal->velocity_accelerator() > kVelocityTolerance) {
+ if (UpToSpeed(goal) &&
+ goal->velocity_finisher() > kVelocityToleranceFinisher &&
+ goal->velocity_accelerator() > kVelocityToleranceAccelerator) {
ready_ = true;
} else {
ready_ = false;
@@ -100,8 +98,8 @@
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);
+ finisher_goal_changed_ = (std::abs(finisher_.velocity() - finisher_goal()) >
+ kVelocityToleranceFinisher);
}
if (!finisher_goal_changed_) {
@@ -115,13 +113,15 @@
const double finisher_velocity_dip =
last_finisher_velocity_max_ - finisher_.velocity();
- if (finisher_velocity_dip < kVelocityTolerance && ball_in_finisher_) {
+ if (finisher_velocity_dip < kVelocityToleranceFinisher &&
+ 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)) {
+ } else if (!ball_in_finisher_ &&
+ (finisher_goal() > kVelocityToleranceFinisher)) {
// There is probably a ball in the flywheel if the angular
// velocity is atleast kMinVelocityErrorWithBall less than the last local
// maximum.
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index f41b882..7a3393b 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -17,6 +17,9 @@
// Handles all flywheels together.
class Shooter {
public:
+ static constexpr double kVelocityToleranceFinisher = 3.0;
+ static constexpr double kVelocityToleranceAccelerator = 4.0;
+
Shooter();
flatbuffers::Offset<ShooterStatus> RunIteration(
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 757e19a..029475a 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -973,7 +973,6 @@
StartSendingFinisherGoals();
- constexpr double kVelocityTolerance = 2.0;
test_event_loop_->AddPhasedLoop(
[&](int) {
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
@@ -982,11 +981,14 @@
->finisher()
->angular_velocity();
const double finisher_velocity_dip = finisher_goal_ - finisher_velocity;
- if (ball_in_finisher_ && finisher_velocity_dip >= kVelocityTolerance) {
+ if (ball_in_finisher_ &&
+ finisher_velocity_dip >=
+ shooter::Shooter::kVelocityToleranceFinisher) {
finisher_velocity_below_goal = true;
}
if (ball_in_finisher_ && finisher_velocity_below_goal &&
- finisher_velocity_dip < kVelocityTolerance) {
+ finisher_velocity_dip <
+ shooter::Shooter::kVelocityToleranceFinisher) {
ball_in_finisher_ = false;
finisher_velocity_below_goal = false;
balls_shot++;
@@ -999,7 +1001,9 @@
balls_shot) ||
((superstructure_status_fetcher_->shooter()->balls_shot() ==
balls_shot + 1) &&
- (finisher_velocity_dip - kVelocityTolerance < 0.1)));
+ (finisher_velocity_dip -
+ shooter::Shooter::kVelocityToleranceFinisher <
+ 0.2)));
},
dt());