Merge "Correctly handle turret-focused image corrections in localizer"
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 28c4bf5..eb5bf74 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 {