Adding a plot for the superstructure
Signed-off-by: Sabina Leaver <100027607@mvla.net>
Change-Id: Ib94a948443015525428145741f9062272ed7ca18
Signed-off-by: Sabina Leaver <100027607@mvla.net>
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 5457405..4850f97 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -52,6 +52,7 @@
"//y2020/control_loops/superstructure:finisher_plotter",
"//y2020/control_loops/superstructure:hood_plotter",
"//y2020/control_loops/superstructure:turret_plotter",
+ "//y2021_bot3/control_loops/superstructure:superstructure_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 7b46a70..2df65f6 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -38,6 +38,8 @@
'org_frc971/y2020/control_loops/superstructure/accelerator_plotter'
import {plotHood} from
'org_frc971/y2020/control_loops/superstructure/hood_plotter'
+import {plotSuperstructure} from
+ 'org_frc971/y2021_bot3/control_loops/superstructure/superstructure_plotter';
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
@@ -104,6 +106,7 @@
['Turret', new PlotState(plotDiv, plotTurret)],
['2020 Localizer', new PlotState(plotDiv, plotLocalizer)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
+ ['Y2021 3rd Robot Superstructure', new PlotState(plotDiv, plotSuperstructure)],
]);
const invalidSelectValue = 'null';
@@ -151,4 +154,4 @@
plotSelect.value = getDefaultPlot();
// Force the event to occur once at the start.
plotSelect.dispatchEvent(new Event('input'));
-});
+});
\ No newline at end of file
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
index 77cdf45..e4dd6a2 100644
--- a/y2021_bot3/control_loops/superstructure/BUILD
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -1,6 +1,7 @@
package(default_visibility = ["//visibility:public"])
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
name = "superstructure_goal_fbs",
@@ -101,4 +102,15 @@
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
],
+)
+
+ts_library(
+ name = "superstructure_plotter",
+ srcs = ["superstructure_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
)
\ No newline at end of file
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
index a6b6353..a7beb91 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -1,13 +1,16 @@
#include <chrono>
#include <memory>
-#include "aos/events/logging/log_writer.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"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2021_bot3/control_loops/superstructure/superstructure.h"
+#include "aos/events/logging/log_writer.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
namespace y2021_bot3 {
namespace control_loops {
@@ -40,6 +43,13 @@
phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
[this](int) { SendPositionMessage(); }, dt());
+
+ if (!FLAGS_output_folder.empty()) {
+ unlink(FLAGS_output_folder.c_str());
+ logger_event_loop_ = MakeEventLoop("logger", roborio_);
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingLocalNamerOnRun(FLAGS_output_folder);
+ }
}
void VerifyResults(double intake_voltage, double outtake_voltage,
@@ -65,6 +75,9 @@
builder.Send(position_builder.Finish());
}
+ // Because the third robot is single node, the roborio node is nullptr
+ const aos::Node *const roborio_ = nullptr;
+
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
::y2021_bot3::control_loops::superstructure::Superstructure superstructure_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -75,6 +88,8 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
::aos::Sender<Position> superstructure_position_sender_;
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
};
// Tests running the intake and outtake separately
@@ -167,6 +182,25 @@
VerifyResults(0.0, 0.0, 0.0, 6.0, 5.0, 4.0);
}
+TEST_F(SuperstructureTest, PlotterTest) {
+ double speed = 10.0;
+ test_event_loop_->AddPhasedLoop(
+ [&](int) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(speed);
+ goal_builder.add_outtake_speed(speed);
+ goal_builder.add_climber_speed(speed);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ speed += .001;
+ if (speed >= 12) {
+ speed = -12;
+ }
+ },
+ frc971::controls::kLoopFrequency);
+ RunFor(std::chrono::seconds(10));
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_plotter.ts b/y2021_bot3/control_loops/superstructure/superstructure_plotter.ts
new file mode 100644
index 0000000..0ceafae
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_plotter.ts
@@ -0,0 +1,45 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+
+export function plotSuperstructure(conn: Connection, element: Element) : void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Position');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ var currentTop = 0;
+
+ const intakePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ intakePlot.plot.getAxisLabels().setTitle('Intake');
+ intakePlot.plot.getAxisLabels().setXLabel(TIME);
+ intakePlot.plot.getAxisLabels().setYLabel('Volts');
+ intakePlot.plot.setDefaultYRange([-20.0, 20.0]);
+
+ intakePlot.addMessageLine(output, ['intake_volts']).setColor(BLUE);
+ intakePlot.addMessageLine(goal, ['intake_speed']).setColor(GREEN);
+ intakePlot.addMessageLine(status, ['intake_speed']).setColor(RED);
+
+ const outtakePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ outtakePlot.plot.getAxisLabels().setTitle('Outtake');
+ outtakePlot.plot.getAxisLabels().setXLabel(TIME);
+ outtakePlot.plot.getAxisLabels().setYLabel('Volts');
+ outtakePlot.plot.setDefaultYRange([-20.0, 20.0]);
+
+ outtakePlot.addMessageLine(output, ['outtake_volts']).setColor(BLUE);
+ outtakePlot.addMessageLine(goal, ['outtake_speed']).setColor(GREEN);
+ outtakePlot.addMessageLine(status, ['outtake_speed']).setColor(RED);
+}