Add 2022 superstructure replay
Also add more info to status and fix plot colors
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I5c9112e013f5e3b75e8f4128db5f35cf9cec336d
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index f67fa12..fabc871 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -130,6 +130,19 @@
],
)
+cc_binary(
+ name = "superstructure_replay",
+ srcs = ["superstructure_replay.cc"],
+ deps = [
+ ":superstructure_lib",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/network:team_number",
+ ],
+)
+
cc_library(
name = "collision_avoidance_lib",
srcs = ["collision_avoidance.cc"],
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 4717f1a..9642ae0 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -34,8 +34,8 @@
event_loop->MakeFetcher<CANPosition>("/superstructure")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- ball_color_fetcher_(event_loop->MakeFetcher<y2022::vision::BallColor>(
- "/superstructure")),
+ ball_color_fetcher_(
+ event_loop->MakeFetcher<y2022::vision::BallColor>("/superstructure")),
aimer_(values) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -601,6 +601,7 @@
status_builder.add_fire(fire_);
status_builder.add_moving_too_fast(moving_too_fast);
status_builder.add_discarding_ball(discarding_ball_);
+ status_builder.add_collided(collided);
status_builder.add_ready_to_fire(state_ == SuperstructureState::LOADED &&
turret_near_goal && !collided);
status_builder.add_state(state_);
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index ec36dd4..5a1537e 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -41,9 +41,24 @@
.setColor(BLUE)
.setPointSize(1.0);
positionPlot.addMessageLine(status, ['fire'])
- .setColor(CYAN)
+ .setColor(BROWN)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['ready_to_fire'])
+ .setColor(GREEN)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['collided'])
+ .setColor(PINK)
.setPointSize(1.0);
+ const shotCountPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ shotCountPlot.plot.getAxisLabels().setTitle('Shot Count');
+ shotCountPlot.plot.getAxisLabels().setXLabel(TIME);
+ shotCountPlot.plot.getAxisLabels().setYLabel('balls');
+ shotCountPlot.plot.setDefaultYRange([-1.0, 2.0]);
+ shotCountPlot.addMessageLine(status, ['shot_count'])
+ .setColor(RED)
+ .setPointSize(1.0);
const intakePlot =
aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
diff --git a/y2022/control_loops/superstructure/superstructure_replay.cc b/y2022/control_loops/superstructure/superstructure_replay.cc
new file mode 100644
index 0000000..b05cdb9
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_replay.cc
@@ -0,0 +1,74 @@
+// This binary allows us to replay the superstructure code over existing logfile.
+// When you run this code, it generates a new logfile with the data all
+// replayed, so that it can then be run through the plotting tool or analyzed
+// in some other way. The original superstructure status data will be on the
+// /original/superstructure channel.
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "aos/network/team_number.h"
+#include "gflags/gflags.h"
+#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/superstructure.h"
+
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/superstructure_replay/",
+ "Logs all channels to the provided logfile.");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ // TODO(james): Actually enforce not sending on the same buses as the logfile
+ // spews out.
+ reader.RemapLoggedChannel("/superstructure",
+ "y2022.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2022.control_loops.superstructure.Output");
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ aos::NodeEventLoopFactory *roborio =
+ factory.GetNodeEventLoopFactory("roborio");
+
+ unlink(FLAGS_output_folder.c_str());
+ std::unique_ptr<aos::EventLoop> logger_event_loop =
+ roborio->MakeEventLoop("logger");
+ auto logger = std::make_unique<aos::logger::Logger>(logger_event_loop.get());
+ logger->StartLoggingOnRun(FLAGS_output_folder);
+
+ roborio->OnStartup([roborio]() {
+ roborio->AlwaysStart<y2022::control_loops::superstructure::Superstructure>(
+ "superstructure", std::make_shared<y2022::constants::Values>(
+ y2022::constants::MakeValues()));
+ });
+
+ std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
+ print_loop->SkipAosLog();
+ print_loop->MakeWatcher(
+ "/aos", [&print_loop](const aos::logging::LogMessageFbs &msg) {
+ LOG(INFO) << print_loop->context().monotonic_event_time << " "
+ << aos::FlatbufferToJson(&msg);
+ });
+ print_loop->MakeWatcher(
+ "/superstructure",
+ [&](const y2022::control_loops::superstructure::Status &status) {
+ if (status.estopped()) {
+ LOG(ERROR) << "Estopped";
+ }
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index ac60ce3..44dc19a 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -55,6 +55,8 @@
flippers_open:bool (id: 12);
// Whether the flippers failed to open and we are retrying
reseating_in_catapult:bool (id: 13);
+ // Whether the turret/catapult is collided with the intake
+ collided:bool(id: 23);
// Whether the turret is ready for firing
ready_to_fire:bool (id: 20);
// Whether the robot is moving too fast to shoot