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/WORKSPACE b/WORKSPACE
index d87fee5..7137327 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -722,6 +722,19 @@
url = "https://www.frc971.org/Build-Dependencies/2021-03-20_drivetrain_spin_wheels.tar.gz",
)
+http_archive(
+ name = "superstructure_replay",
+ build_file_content = """
+filegroup(
+ name = "superstructure_replay",
+ srcs = glob(["**/*.bfbs"]),
+ visibility = ["//visibility:public"],
+)
+ """,
+ sha256 = "e7037a374a7bf315b25058f0e9fbbf3e99c425e3a8e7d51edfc50e3f2a36a182",
+ url = "https://www.frc971.org/Build-Dependencies/2021-09-04_superstructure_shoot_balls.tar.gz",
+)
+
# OpenCV armhf (for raspberry pi)
http_archive(
name = "opencv_armhf",
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index db8a060..a39f3df 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -108,6 +108,7 @@
],
data = [
"//y2020:config",
+ "@superstructure_replay",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
@@ -117,7 +118,9 @@
":superstructure_position_fbs",
":superstructure_status_fbs",
"//aos:math",
+ "//aos/events/logging:log_reader",
"//aos/events/logging:log_writer",
+ "//aos/network:team_number",
"//aos/testing:googletest",
"//aos/time",
"//frc971/control_loops:capped_test_plant",
diff --git a/y2020/control_loops/superstructure/finisher_plotter.ts b/y2020/control_loops/superstructure/finisher_plotter.ts
index 6ec4be4..c9420ae 100644
--- a/y2020/control_loops/superstructure/finisher_plotter.ts
+++ b/y2020/control_loops/superstructure/finisher_plotter.ts
@@ -34,6 +34,16 @@
velocityPlot.addMessageLine(status, ['shooter', 'ready']).setColor(BLACK).setPointSize(0.0);
velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'dt_angular_velocity']).setColor(PINK).setPointSize(0.0);
+ const ballsShotPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ ballsShotPlot.plot.getAxisLabels().setTitle('Balls Shot');
+ ballsShotPlot.plot.getAxisLabels().setXLabel(TIME);
+ ballsShotPlot.plot.getAxisLabels().setYLabel('Balls');
+ ballsShotPlot.plot.setDefaultYRange([0.0, 20.0]);
+ ballsShotPlot.addMessageLine(status, ['shooter', 'balls_shot']).setColor(BLUE).setPointSize(0.0);
+
+
const voltagePlot =
aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentTop += DEFAULT_HEIGHT / 2;
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);
};
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index f71fe17..4e2dc88 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -3,7 +3,9 @@
#include <chrono>
#include <memory>
+#include "aos/events/logging/log_reader.h"
#include "aos/events/logging/log_writer.h"
+#include "aos/network/team_number.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/position_sensor_sim.h"
@@ -18,6 +20,10 @@
DEFINE_string(output_file, "",
"If set, logs all channels to the provided logfile.");
+DEFINE_string(replay_logfile, "external/superstructure_replay/",
+ "Name of the logfile to read from and replay.");
+DEFINE_string(config, "y2020/config.json",
+ "Name of the config file to replay using.");
namespace y2020 {
namespace control_loops {
@@ -374,6 +380,9 @@
peak_turret_acceleration_ = value;
}
void set_peak_turret_velocity(double value) { peak_turret_velocity_ = value; }
+ void set_finisher_voltage_offset(double value) {
+ finisher_plant_->set_voltage_offset(value);
+ }
private:
::aos::EventLoop *event_loop_;
@@ -444,7 +453,7 @@
unlink(FLAGS_output_file.c_str());
logger_event_loop_ = MakeEventLoop("logger", roborio_);
logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
- logger_->StartLoggingLocalNamerOnRun(FLAGS_output_file);
+ logger_->StartLoggingOnRun(FLAGS_output_file);
}
}
@@ -552,6 +561,47 @@
expected_voltage);
}
+ void StartSendingFinisherGoals() {
+ test_event_loop_->AddPhasedLoop(
+ [this](int) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ auto shooter_goal_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_goal_builder.add_velocity_finisher(finisher_goal_);
+ const auto shooter_goal_offset = shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shooter(shooter_goal_offset);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ },
+ dt());
+ }
+
+ // Sets the finisher velocity goal (radians/s)
+ void SetFinisherGoalAfter(const double velocity_finisher,
+ const monotonic_clock::duration time_offset) {
+ test_event_loop_
+ ->AddTimer(
+ [this, velocity_finisher] { finisher_goal_ = velocity_finisher; })
+ ->Setup(test_event_loop_->monotonic_now() + time_offset);
+ }
+
+ // Simulates the friction of a ball in the flywheel
+ void ApplyFrictionToFinisherAfter(
+ const double voltage_offset, const bool ball_in_finisher,
+ const monotonic_clock::duration time_offset) {
+ test_event_loop_
+ ->AddTimer([this, voltage_offset, ball_in_finisher] {
+ superstructure_plant_.set_finisher_voltage_offset(voltage_offset);
+ ball_in_finisher_ = ball_in_finisher;
+ })
+ ->Setup(test_event_loop_->monotonic_now() + time_offset);
+ test_event_loop_
+ ->AddTimer(
+ [this] { superstructure_plant_.set_finisher_voltage_offset(0); })
+ ->Setup(test_event_loop_->monotonic_now() + time_offset +
+ chrono::seconds(1));
+ }
+
const aos::Node *const roborio_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -573,6 +623,9 @@
std::unique_ptr<aos::EventLoop> logger_event_loop_;
std::unique_ptr<aos::logger::Logger> logger_;
+
+ double finisher_goal_ = 0;
+ bool ball_in_finisher_ = false;
};
// Tests that the superstructure does nothing when the goal is to remain
@@ -907,6 +960,148 @@
TestIntakeRollerVoltage(0.0f, 0.0f, true, 3.0);
}
+// Tests that we detect that a ball was shot whenever the average angular
+// velocity is lower than a certain threshold compared to the goal.
+TEST_F(SuperstructureTest, BallsShot) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ int balls_shot = 0;
+ // When there is a ball in the flywheel, the finisher velocity should drop
+ // below the goal
+ bool finisher_velocity_below_goal = false;
+
+ StartSendingFinisherGoals();
+
+ constexpr double kVelocityTolerance = 2.0;
+ test_event_loop_->AddPhasedLoop(
+ [&](int) {
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ const double finisher_velocity =
+ superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity();
+ const double finisher_velocity_dip = finisher_goal_ - finisher_velocity;
+ if (ball_in_finisher_ && finisher_velocity_dip >= kVelocityTolerance) {
+ finisher_velocity_below_goal = true;
+ }
+ if (ball_in_finisher_ && finisher_velocity_below_goal &&
+ finisher_velocity_dip < kVelocityTolerance) {
+ ball_in_finisher_ = false;
+ finisher_velocity_below_goal = false;
+ balls_shot++;
+ }
+ // Since here we are calculating the dip from the goal instead of the
+ // local maximum, the shooter could have calculated that a ball was shot
+ // slightly before we did if the local maximum was slightly below the
+ // goal.
+ EXPECT_TRUE((superstructure_status_fetcher_->shooter()->balls_shot() ==
+ balls_shot) ||
+ ((superstructure_status_fetcher_->shooter()->balls_shot() ==
+ balls_shot + 1) &&
+ (finisher_velocity_dip - kVelocityTolerance < 0.1)));
+ },
+ dt());
+
+ constexpr int kFastShootingSpeed = 500;
+ constexpr int kSlowShootingSpeed = 300;
+
+ // Maximum (since it is negative) flywheel voltage offsets for simulating the
+ // friction of a ball at different finisher speeds.
+ // Slower speeds require a higher magnitude of voltage offset.
+ static constexpr double kFastSpeedVoltageOffsetWithBall = -4.1;
+ static constexpr double kSlowSpeedVoltageOffsetWithBall = -4.5;
+
+ SetFinisherGoalAfter(kFastShootingSpeed, chrono::seconds(1));
+ // Simulate shooting balls by applying friction to the finisher
+ ApplyFrictionToFinisherAfter(kFastSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(3));
+ ApplyFrictionToFinisherAfter(kFastSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(6));
+
+ SetFinisherGoalAfter(0, chrono::seconds(10));
+ SetFinisherGoalAfter(kFastShootingSpeed, chrono::seconds(15));
+ ApplyFrictionToFinisherAfter(kFastSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(18));
+ ApplyFrictionToFinisherAfter(kFastSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(21));
+
+ SetFinisherGoalAfter(kSlowShootingSpeed, chrono::seconds(25));
+ ApplyFrictionToFinisherAfter(kSlowSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(28));
+ ApplyFrictionToFinisherAfter(kSlowSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(31));
+ // This smaller decrease in velocity shouldn't be counted as a ball
+ ApplyFrictionToFinisherAfter(kSlowSpeedVoltageOffsetWithBall / 2, false,
+ chrono::seconds(34));
+
+ SetFinisherGoalAfter(kFastShootingSpeed, chrono::seconds(38));
+ ApplyFrictionToFinisherAfter(kFastSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(41));
+ ApplyFrictionToFinisherAfter(kFastSpeedVoltageOffsetWithBall, true,
+ chrono::seconds(44));
+ // This slow positive voltage offset that speeds up the flywheel instead of
+ // slowing it down shouldn't be counted as a ball.
+ // We wouldn't expect a positive voltage offset of more than ~2 volts.
+ ApplyFrictionToFinisherAfter(2, false, chrono::seconds(47));
+
+ RunFor(chrono::seconds(50));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->balls_shot(), 8);
+}
+
+class SuperstructureReplayTest : public ::testing::Test {
+ public:
+ SuperstructureReplayTest()
+ : config_(aos::configuration::ReadConfig(FLAGS_config)),
+ reader_(
+ aos::logger::SortParts(aos::logger::FindLogs(FLAGS_replay_logfile)),
+ &config_.message()) {
+ aos::network::OverrideTeamNumber(971);
+
+ reader_.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Status");
+ reader_.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Output");
+ reader_.Register();
+
+ roborio_ = aos::configuration::GetNode(reader_.configuration(), "roborio");
+
+ superstructure_event_loop_ =
+ reader_.event_loop_factory()->MakeEventLoop("superstructure", roborio_);
+ superstructure_event_loop_->SkipTimingReport();
+
+ test_event_loop_ = reader_.event_loop_factory()->MakeEventLoop(
+ "superstructure_replay_test", roborio_);
+
+ status_fetcher_ =
+ test_event_loop_
+ ->MakeFetcher<y2020::control_loops::superstructure::Status>(
+ "/superstructure");
+ }
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::logger::LogReader reader_;
+ const aos::Node *roborio_;
+
+ std::unique_ptr<aos::EventLoop> superstructure_event_loop_;
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+
+ aos::Fetcher<y2020::control_loops::superstructure::Status> status_fetcher_;
+};
+
+// Tests that balls_shot is updated correctly with a real log
+// that had target tracking constantly changing the finisher goal by small
+// amounts.
+TEST_F(SuperstructureReplayTest, BallsShotWithTargetTracking) {
+ Superstructure superstructure(superstructure_event_loop_.get());
+ reader_.event_loop_factory()->Run();
+
+ ASSERT_TRUE(status_fetcher_.Fetch());
+ EXPECT_EQ(status_fetcher_->shooter()->balls_shot(), 3);
+}
+
class SuperstructureAllianceTest
: public SuperstructureTest,
public ::testing::WithParamInterface<aos::Alliance> {
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index e55e19b..aab1341 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -37,6 +37,9 @@
// If the shooter is ready, this is true. Note: don't use this for status
// checking, only for plotting. Changes in goal take time to impact this.
ready:bool (id: 3);
+
+ // The total number of balls we have shot
+ balls_shot:int (id: 4);
}
table AimerStatus {