Merge "Switch to using 128 bit math for time interpolation"
diff --git a/aos/network/BUILD b/aos/network/BUILD
index bf2cb00..6053260 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -389,11 +389,11 @@
hdrs = ["web_proxy.h"],
copts = [
"-DWEBRTC_POSIX",
- "-Wno-unused-parameter",
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
":connect_fbs",
+ ":gen_embedded",
":web_proxy_fbs",
":web_proxy_utils",
"//aos/events:shm_event_loop",
@@ -419,7 +419,6 @@
srcs = ["web_proxy_main.cc"],
copts = [
"-DWEBRTC_POSIX",
- "-Wno-unused-parameter",
],
data = [
"//aos/network/www:files",
@@ -428,12 +427,9 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
- ":gen_embedded",
":web_proxy",
"//aos:init",
"//aos/events:shm_event_loop",
- "//aos/seasocks:seasocks_logger",
- "//third_party/seasocks",
"@com_github_google_flatbuffers//:flatbuffers",
],
)
@@ -446,16 +442,12 @@
],
copts = [
"-DWEBRTC_POSIX",
- "-Wno-unused-parameter",
],
deps = [
- ":gen_embedded",
":web_proxy",
"//aos:init",
"//aos/events:simulated_event_loop",
"//aos/events/logging:logger",
- "//aos/seasocks:seasocks_logger",
- "//third_party/seasocks",
"@com_github_google_flatbuffers//:flatbuffers",
],
)
diff --git a/aos/network/log_web_proxy_main.cc b/aos/network/log_web_proxy_main.cc
index dda9ae4..1942c57 100644
--- a/aos/network/log_web_proxy_main.cc
+++ b/aos/network/log_web_proxy_main.cc
@@ -9,21 +9,14 @@
#include "aos/flatbuffer_merge.h"
#include "aos/init.h"
#include "aos/network/web_proxy.h"
-#include "aos/seasocks/seasocks_logger.h"
#include "gflags/gflags.h"
-#include "internal/Embedded.h"
-#include "seasocks/Server.h"
-#include "seasocks/WebSocket.h"
-
DEFINE_string(data_dir, "www", "Directory to serve data files from");
DEFINE_string(node, "", "Directory to serve data files from");
DEFINE_int32(buffer_size, -1, "-1 if infinite, in # of messages / channel.");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
- // Make sure to reference this to force the linker to include it.
- findEmbeddedContent("");
const std::vector<std::string> unsorted_logfiles =
aos::logger::FindLogs(argc, argv);
diff --git a/aos/network/web_proxy.cc b/aos/network/web_proxy.cc
index af1b646..5902d30 100644
--- a/aos/network/web_proxy.cc
+++ b/aos/network/web_proxy.cc
@@ -7,6 +7,7 @@
#include "aos/seasocks/seasocks_logger.h"
#include "api/create_peerconnection_factory.h"
#include "glog/logging.h"
+#include "internal/Embedded.h"
namespace aos {
namespace web_proxy {
@@ -22,7 +23,7 @@
return new rtc::RefCountedObject<DummySetSessionDescriptionObserver>();
}
virtual void OnSuccess() {}
- virtual void OnFailure(webrtc::RTCError error) {}
+ virtual void OnFailure(webrtc::RTCError /*error*/) {}
};
} // namespace
@@ -32,6 +33,8 @@
: server_(server),
config_(aos::CopyFlatBuffer(event_loop->configuration())),
event_loop_(event_loop) {
+ // We need to reference findEmbeddedContent() to make the linker happy...
+ findEmbeddedContent("");
const aos::Node *self = event_loop->node();
for (uint i = 0; i < event_loop->configuration()->channels()->size(); ++i) {
@@ -276,11 +279,14 @@
// Function called for web socket data. Parses the flatbuffer and
// handles it appropriately.
void Connection::HandleWebSocketData(const uint8_t *data, size_t size) {
- const WebSocketMessage *message =
- flatbuffers::GetRoot<WebSocketMessage>(data);
- switch (message->payload_type()) {
+ const FlatbufferSpan<WebSocketMessage> message({data, size});
+ if (!message.Verify()) {
+ LOG(ERROR) << "Invalid WebsocketMessage received from browser.";
+ return;
+ }
+ switch (message.message().payload_type()) {
case Payload::WebSocketSdp: {
- const WebSocketSdp *offer = message->payload_as_WebSocketSdp();
+ const WebSocketSdp *offer = message.message().payload_as_WebSocketSdp();
if (offer->type() != SdpType::OFFER) {
LOG(WARNING) << "Got the wrong sdp type from client";
break;
@@ -325,7 +331,7 @@
break;
}
case Payload::WebSocketIce: {
- const WebSocketIce *ice = message->payload_as_WebSocketIce();
+ const WebSocketIce *ice = message.message().payload_as_WebSocketIce();
std::string candidate = ice->candidate()->str();
std::string sdpMid = ice->sdpMid()->str();
int sdpMLineIndex = ice->sdpMLineIndex();
diff --git a/aos/network/web_proxy.h b/aos/network/web_proxy.h
index ab524de..e6d47c4 100644
--- a/aos/network/web_proxy.h
+++ b/aos/network/web_proxy.h
@@ -178,7 +178,7 @@
rtc::scoped_refptr<webrtc::DataChannelInterface> channel) override;
void OnRenegotiationNeeded() override {}
void OnIceConnectionChange(
- webrtc::PeerConnectionInterface::IceConnectionState state) override {}
+ webrtc::PeerConnectionInterface::IceConnectionState /*state*/) override {}
void OnIceGatheringChange(
webrtc::PeerConnectionInterface::IceGatheringState) override {}
void OnIceCandidate(const webrtc::IceCandidateInterface *candidate) override;
@@ -186,7 +186,7 @@
// CreateSessionDescriptionObserver implementation
void OnSuccess(webrtc::SessionDescriptionInterface *desc) override;
- void OnFailure(webrtc::RTCError error) override {}
+ void OnFailure(webrtc::RTCError /*error*/) override {}
// CreateSessionDescriptionObserver is a refcounted object
void AddRef() const override {}
// We handle ownership with a unique_ptr so don't worry about actually
@@ -198,7 +198,7 @@
// DataChannelObserver implementation
void OnStateChange() override;
void OnMessage(const webrtc::DataBuffer &buffer) override;
- void OnBufferedAmountChange(uint64_t sent_data_size) override {}
+ void OnBufferedAmountChange(uint64_t /*sent_data_size*/) override {}
private:
::seasocks::WebSocket *sock_;
diff --git a/aos/network/web_proxy_main.cc b/aos/network/web_proxy_main.cc
index ddab5dc..06fe942 100644
--- a/aos/network/web_proxy_main.cc
+++ b/aos/network/web_proxy_main.cc
@@ -2,21 +2,14 @@
#include "aos/flatbuffer_merge.h"
#include "aos/init.h"
#include "aos/network/web_proxy.h"
-#include "aos/seasocks/seasocks_logger.h"
#include "gflags/gflags.h"
-#include "internal/Embedded.h"
-#include "seasocks/Server.h"
-#include "seasocks/WebSocket.h"
-
DEFINE_string(config, "./config.json", "File path of aos configuration");
DEFINE_string(data_dir, "www", "Directory to serve data files from");
DEFINE_int32(buffer_size, 0, "-1 if infinite, in # of messages / channel.");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
- // Make sure to reference this to force the linker to include it.
- findEmbeddedContent("");
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 948f8db..02573b3 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -125,6 +125,12 @@
const request = new ChannelRequest(channel, method);
if (!this.handlerFuncs.has(channel.key())) {
this.handlerFuncs.set(channel.key(), []);
+ } else {
+ if (method == TransferMethod.EVERYTHING_WITH_HISTORY) {
+ console.warn(
+ 'Behavior of multiple reliable handlers is currently poorly ' +
+ 'defined and may not actually deliver all of the messages.');
+ }
}
this.handlerFuncs.get(channel.key()).push(handler);
this.subscribeToChannel(request);
diff --git a/aos/network/www/reflection.ts b/aos/network/www/reflection.ts
index 2e1ed8f..09206e0 100644
--- a/aos/network/www/reflection.ts
+++ b/aos/network/www/reflection.ts
@@ -246,7 +246,9 @@
return field;
}
}
- throw new Error('Couldn\'t find field ' + fieldName + '.');
+ throw new Error(
+ 'Couldn\'t find field ' + fieldName + ' in object ' + schema.name() +
+ '.');
}
// Reads a scalar with the given field name from a Table. If readDefaults
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 465df0d..f378b68 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -3,6 +3,8 @@
load("@npm_bazel_typescript//:defs.bzl", "ts_library")
load("@com_google_protobuf//:protobuf.bzl", "py_proto_library")
load("@build_bazel_rules_nodejs//:defs.bzl", "nodejs_binary", "rollup_bundle")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
+load("//aos:config.bzl", "aos_config")
py_binary(
name = "plot_action",
@@ -89,9 +91,11 @@
srcs = ["plot_index.ts"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ ":plot_data_utils",
"//aos:configuration_ts_fbs",
"//aos/network/www:demo_plot",
"//aos/network/www:proxy",
+ "//frc971/control_loops/drivetrain:drivetrain_plotter",
"//frc971/wpilib:imu_plotter",
],
)
@@ -134,3 +138,75 @@
],
target_compatible_with = ["@platforms//os:linux"],
)
+
+flatbuffer_cc_library(
+ name = "plot_data_fbs",
+ srcs = [
+ "plot_data.fbs",
+ ],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+flatbuffer_ts_library(
+ name = "plot_data_ts_fbs",
+ srcs = [
+ "plot_data.fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+ts_library(
+ name = "plot_data_utils",
+ srcs = ["plot_data_utils.ts"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":plot_data_ts_fbs",
+ "//aos:configuration_ts_fbs",
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:plotter",
+ "//aos/network/www:proxy",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+aos_config(
+ name = "plotter",
+ src = "plotter_config.json",
+ flatbuffers = [":plot_data_fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//aos/events:config"],
+)
+
+cc_library(
+ name = "in_process_plotter",
+ srcs = ["in_process_plotter.cc"],
+ hdrs = ["in_process_plotter.h"],
+ copts = [
+ "-DWEBRTC_POSIX",
+ ],
+ deps = [
+ ":plot_data_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/network:web_proxy",
+ ],
+)
+
+cc_binary(
+ name = "in_process_plotter_demo",
+ srcs = ["in_process_plotter_demo.cc"],
+ copts = [
+ "-DWEBRTC_POSIX",
+ ],
+ data = [
+ ":plotter",
+ ":plotter_files",
+ ],
+ # Tagged manual until we either get the linker working with the current
+ # WebRTC implementation or we get a new implementation.
+ tags = ["manual"],
+ deps = [
+ ":in_process_plotter",
+ "//aos:init",
+ ],
+)
diff --git a/frc971/analysis/in_process_plotter.cc b/frc971/analysis/in_process_plotter.cc
new file mode 100644
index 0000000..e82cb2f
--- /dev/null
+++ b/frc971/analysis/in_process_plotter.cc
@@ -0,0 +1,131 @@
+#include "frc971/analysis/in_process_plotter.h"
+
+#include "aos/configuration.h"
+
+namespace frc971 {
+namespace analysis {
+
+namespace {
+const char *kDataPath = "frc971/analysis";
+const char *kConfigPath = "frc971/analysis/plotter.json";
+} // namespace
+
+Plotter::Plotter()
+ : config_(aos::configuration::ReadConfig(kConfigPath)),
+ event_loop_factory_(&config_.message()),
+ event_loop_(event_loop_factory_.MakeEventLoop("plotter")),
+ plot_sender_(event_loop_->MakeSender<Plot>("/analysis")),
+ web_proxy_(event_loop_.get(), -1),
+ builder_(plot_sender_.MakeBuilder()) {
+ web_proxy_.SetDataPath(kDataPath);
+ event_loop_->SkipTimingReport();
+ color_wheel_.push_back(Color(1, 0, 0));
+ color_wheel_.push_back(Color(0, 1, 0));
+ color_wheel_.push_back(Color(0, 0, 1));
+ color_wheel_.push_back(Color(1, 1, 0));
+ color_wheel_.push_back(Color(0, 1, 1));
+ color_wheel_.push_back(Color(1, 0, 1));
+}
+
+void Plotter::Spin() { event_loop_factory_.Run(); }
+
+void Plotter::Title(std::string_view title) {
+ title_ = builder_.fbb()->CreateString(title);
+}
+
+void Plotter::AddFigure(std::string_view title, double width, double height) {
+ MaybeFinishFigure();
+
+ if (!title.empty()) {
+ figure_title_ = builder_.fbb()->CreateString(title);
+ }
+
+ // For positioning, just stack figures vertically.
+ auto position_builder = builder_.MakeBuilder<Position>();
+ position_builder.add_top(next_top_);
+ position_builder.add_left(0);
+ position_builder.add_width(width);
+ position_builder.add_height(height);
+ position_ = position_builder.Finish();
+
+ next_top_ += height;
+}
+
+void Plotter::XLabel(std::string_view label) {
+ xlabel_ = builder_.fbb()->CreateString(label);
+}
+
+void Plotter::YLabel(std::string_view label) {
+ ylabel_ = builder_.fbb()->CreateString(label);
+}
+
+void Plotter::AddLine(const std::vector<double> &x,
+ const std::vector<double> &y, std::string_view label) {
+ CHECK_EQ(x.size(), y.size());
+ CHECK(!position_.IsNull())
+ << "You must call AddFigure() before calling AddLine().";
+
+ flatbuffers::Offset<flatbuffers::String> label_offset;
+ if (!label.empty()) {
+ label_offset = builder_.fbb()->CreateString(label);
+ }
+
+ std::vector<Point> points;
+ for (size_t ii = 0; ii < x.size(); ++ii) {
+ points.emplace_back(x[ii], y[ii]);
+ }
+ const flatbuffers::Offset<flatbuffers::Vector<const Point*>>
+ points_offset = builder_.fbb()->CreateVectorOfStructs(points);
+
+ const Color *color = &color_wheel_.at(color_wheel_position_);
+ color_wheel_position_ = (color_wheel_position_ + 1) % color_wheel_.size();
+
+ auto line_builder = builder_.MakeBuilder<Line>();
+ line_builder.add_label(label_offset);
+ line_builder.add_points(points_offset);
+ line_builder.add_color(color);
+ lines_.push_back(line_builder.Finish());
+}
+
+void Plotter::MaybeFinishFigure() {
+ if (!lines_.empty()) {
+ const flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Line>>>
+ lines_offset = builder_.fbb()->CreateVector(lines_);
+ auto figure_builder = builder_.MakeBuilder<Figure>();
+ figure_builder.add_title(figure_title_);
+ figure_builder.add_position(position_);
+ figure_builder.add_lines(lines_offset);
+ figure_builder.add_xlabel(xlabel_);
+ figure_builder.add_share_x_axis(share_x_axis_);
+ figure_builder.add_ylabel(ylabel_);
+ figures_.push_back(figure_builder.Finish());
+ }
+ lines_.clear();
+ figure_title_.o = 0;
+ xlabel_.o = 0;
+ ylabel_.o = 0;
+ position_.o = 0;
+ share_x_axis_ = false;
+ color_wheel_position_ = 0;
+}
+
+void Plotter::Publish() {
+ MaybeFinishFigure();
+ const flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Figure>>>
+ figures_offset = builder_.fbb()->CreateVector(figures_);
+
+ auto plot_builder = builder_.MakeBuilder<Plot>();
+ plot_builder.add_title(title_);
+ plot_builder.add_figures(figures_offset);
+
+ builder_.Send(plot_builder.Finish());
+
+ builder_ = plot_sender_.MakeBuilder();
+
+ title_.o = 0;
+ figures_.clear();
+ next_top_ = 0;
+}
+
+} // namespace analysis
+} // namespace frc971
diff --git a/frc971/analysis/in_process_plotter.h b/frc971/analysis/in_process_plotter.h
new file mode 100644
index 0000000..3d7a037
--- /dev/null
+++ b/frc971/analysis/in_process_plotter.h
@@ -0,0 +1,79 @@
+#ifndef FRC971_ANALYSIS_IN_PROCESS_PLOTTER_H_
+#define FRC971_ANALYSIS_IN_PROCESS_PLOTTER_H_
+
+#include <vector>
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/network/web_proxy.h"
+#include "frc971/analysis/plot_data_generated.h"
+
+namespace frc971 {
+namespace analysis {
+
+// This class wraps the WebProxy class to provide a convenient C++ interface to
+// dynamically generate plots.
+// Currently, the main useful interface that this provides is a matplotlib-like
+// interface--see in_process_plotter_demo.cc for sample usage. It doesn't
+// precisely follow matplotlib's conventions, but the basic style does mimic
+// matplotlib. Future iterations may ditch this in favor of a more modern
+// interface where we actually return handles for plots and lines and the such.
+//
+// Note that currently the port for the seb server is hard-coded to 8080, so
+// only one instance of the Plotter can be present at once.
+//
+// You must call Spin() for the web server to actually do anything helpful.
+class Plotter {
+ public:
+ Plotter();
+
+ // matplotlib-like interface
+ // The basic pattern is:
+ // 1) Call Figure()
+ // 2) Setup the lines, labels, etc. for the figure.
+ // 3) Repeat 1-2 however many times.
+ // 4) Call Publish().
+ // 5) Repeat 1-5 however many times.
+ //
+ // Publish() actually pushes the figures that you setup to the web-page,
+ // either with an autogenerated title or the title set by Title(). All state
+ // is cleared (or should be cleared) by the call to Publish().
+
+ // Sets the title for the current set of plots; if you
+ void Title(std::string_view title);
+ void AddFigure(std::string_view title = "", double width = 900,
+ double height = 400);
+ void AddLine(const std::vector<double> &x, const std::vector<double> &y,
+ std::string_view label = "");
+ void ShareXAxis(bool share) { share_x_axis_ = share; }
+ void XLabel(std::string_view label);
+ void YLabel(std::string_view label);
+ void Publish();
+
+ void Spin();
+ private:
+ void MaybeFinishFigure();
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ std::unique_ptr<aos::EventLoop> event_loop_;
+ aos::Sender<Plot> plot_sender_;
+ aos::web_proxy::WebProxy web_proxy_;
+
+ aos::Sender<Plot>::Builder builder_;
+ flatbuffers::Offset<flatbuffers::String> title_;
+ flatbuffers::Offset<flatbuffers::String> figure_title_;
+ flatbuffers::Offset<flatbuffers::String> xlabel_;
+ flatbuffers::Offset<flatbuffers::String> ylabel_;
+ bool share_x_axis_ = false;
+ float next_top_ = 0;
+ flatbuffers::Offset<Position> position_;
+ std::vector<flatbuffers::Offset<Figure>> figures_;
+ std::vector<flatbuffers::Offset<Line>> lines_;
+
+ size_t color_wheel_position_ = 0;
+ std::vector<Color> color_wheel_;
+};
+
+} // namespace analysis
+} // namespace frc971
+#endif // FRC971_ANALYSIS_IN_PROCESS_PLOTTER_H_
diff --git a/frc971/analysis/in_process_plotter_demo.cc b/frc971/analysis/in_process_plotter_demo.cc
new file mode 100644
index 0000000..492ddfa
--- /dev/null
+++ b/frc971/analysis/in_process_plotter_demo.cc
@@ -0,0 +1,42 @@
+#include "frc971/analysis/in_process_plotter.h"
+
+#include "aos/init.h"
+
+// To run this example, do:
+// bazel run -c opt //frc971/analysis:in_process_plotter_demo
+// And then open localhost:8080, select "C++ Plotter" from the drop-down, and
+// then select "TITLE!" or "Trig Functions" from the second drop-down to see
+// each plot.
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+ frc971::analysis::Plotter plotter;
+ plotter.Title("TITLE!");
+ plotter.AddFigure("Fig Foo");
+ plotter.ShareXAxis(true);
+ plotter.AddLine({1, 2, 3, 4, 5}, {1, 2, 3, 4, 5}, "y = x");
+ plotter.AddLine({5, 4, 3, 2, 1}, {1, 2, 3, 4, 5}, "y = -x");
+ plotter.YLabel("Y Axis");
+ plotter.AddFigure("Fig Bar");
+ plotter.ShareXAxis(true);
+ plotter.AddLine({1, 2, 3}, {3, 4, 5}, "y = x + 2");
+ plotter.XLabel("X Axis (Linked to both above plots)");
+ plotter.Publish();
+
+ plotter.Title("Trig Functions");
+
+ plotter.AddFigure("Sin & Cos");
+ std::vector<double> x;
+ std::vector<double> sinx;
+ std::vector<double> cosx;
+ constexpr int kNumPoints = 100000;
+ for (int ii = 0; ii < kNumPoints; ++ii) {
+ x.push_back(ii * 2 * M_PI / kNumPoints);
+ sinx.push_back(std::sin(x.back()));
+ cosx.push_back(std::cos(x.back()));
+ }
+ plotter.AddLine(x, sinx, "sin(x)");
+ plotter.AddLine(x, cosx, "cos(x)");
+ plotter.Publish();
+
+ plotter.Spin();
+}
diff --git a/frc971/analysis/plot_data.fbs b/frc971/analysis/plot_data.fbs
new file mode 100644
index 0000000..c21add1
--- /dev/null
+++ b/frc971/analysis/plot_data.fbs
@@ -0,0 +1,53 @@
+// This flatbuffer defines the interface that is used by the in-process
+// web plotter to plot data dynamically. Both the structure of the plot and
+// the data to plot is all packaged within a single Plot message. Each Plot
+// message will correspond to a single view/tab on the web-page, and can have
+// multiple figures, each of which can have multiple lines.
+namespace frc971.analysis;
+
+// Position within the web-page to plot a figure at. [0, 0] will be the upper
+// left corner of the allowable places where plots can be put, and should
+// generally be the default location. All values in pixels.
+table Position {
+ top:float (id: 0);
+ left:float (id: 1);
+ width:float (id: 2);
+ height:float (id: 3);
+}
+
+struct Point {
+ x:double (id: 0);
+ y:double (id: 1);
+}
+
+// RGB values are in the range [0, 1].
+struct Color {
+ r:float (id: 0);
+ g:float (id: 1);
+ b:float (id: 2);
+}
+
+table Line {
+ label:string (id: 0);
+ points:[Point] (id: 1);
+ color:Color (id: 2);
+}
+
+table Figure {
+ position:Position (id: 0);
+ lines:[Line] (id: 1);
+ // Specifies whether to link the x-axis of this Figure with that of other
+ // figures in this Plot. Only the axes of Figure's with this flag set will
+ // be linked.
+ share_x_axis:bool (id: 2);
+ title:string (id: 3);
+ xlabel:string (id: 4);
+ ylabel:string (id: 5);
+}
+
+table Plot {
+ figures:[Figure] (id: 0);
+ title:string (id: 1);
+}
+
+root_type Plot;
diff --git a/frc971/analysis/plot_data_utils.ts b/frc971/analysis/plot_data_utils.ts
new file mode 100644
index 0000000..8d42a4a
--- /dev/null
+++ b/frc971/analysis/plot_data_utils.ts
@@ -0,0 +1,95 @@
+// Provides a plot which handles plotting the plot defined by a
+// frc971.analysis.Plot message.
+import * as configuration from 'org_frc971/aos/configuration_generated';
+import * as plot_data from 'org_frc971/frc971/analysis/plot_data_generated';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
+import {Plot} from 'org_frc971/aos/network/www/plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+import Schema = configuration.reflection.Schema;
+import PlotFb = plot_data.frc971.analysis.Plot;
+
+export function plotData(conn: Connection, parentDiv: Element) {
+ // Set up a selection box to allow the user to choose between plots to show.
+ const plotSelect = document.createElement('select');
+ parentDiv.appendChild(plotSelect);
+ const plots = new Map<string, HTMLElement>();
+ const invalidSelectValue = 'null';
+ plotSelect.addEventListener('input', () => {
+ for (const plot of plots.values()) {
+ plot.style.display = 'none';
+ }
+ if (plotSelect.value == invalidSelectValue) {
+ return;
+ }
+ plots.get(plotSelect.value).style.display = 'block';
+ });
+ plotSelect.add(new Option('Select Plot', invalidSelectValue));
+
+ const plotDiv = document.createElement('div');
+ plotDiv.style.position = 'absolute';
+ plotDiv.style.top = '30';
+ plotDiv.style.left = '0';
+ parentDiv.appendChild(plotDiv);
+
+ conn.addReliableHandler(
+ '/analysis', 'frc971.analysis.Plot', (data: Uint8Array, time: number) => {
+ const plotFb = PlotFb.getRootAsPlot(
+ new ByteBuffer(data) as unknown as flatbuffers.ByteBuffer);
+ const name = (!plotFb.title()) ? 'Plot ' + plots.size : plotFb.title();
+ const div = document.createElement('div');
+ div.style.display = 'none';
+ plots.set(name, div);
+ plotDiv.appendChild(div);
+ plotSelect.add(new Option(name, name));
+
+ const linkedXAxes: Plot[] = [];
+
+ for (let ii = 0; ii < plotFb.figuresLength(); ++ii) {
+ const figure = plotFb.figures(ii);
+ const figureDiv = document.createElement('div');
+ figureDiv.style.top = figure.position().top().toString();
+ figureDiv.style.left = figure.position().left().toString();
+ figureDiv.style.position = 'absolute';
+ div.appendChild(figureDiv);
+ const plot = new Plot(
+ figureDiv, figure.position().width(), figure.position().height());
+
+ if (figure.title()) {
+ plot.getAxisLabels().setTitle(figure.title());
+ }
+ if (figure.xlabel()) {
+ plot.getAxisLabels().setXLabel(figure.xlabel());
+ }
+ if (figure.ylabel()) {
+ plot.getAxisLabels().setYLabel(figure.xlabel());
+ }
+ if (figure.shareXAxis()) {
+ for (const other of linkedXAxes) {
+ plot.linkXAxis(other);
+ }
+ linkedXAxes.push(plot);
+ }
+
+ for (let jj = 0; jj < figure.linesLength(); ++jj) {
+ const lineFb = figure.lines(jj);
+ const line = plot.getDrawer().addLine();
+ if (lineFb.label()) {
+ line.setLabel(lineFb.label());
+ }
+ const points = new Float32Array(lineFb.pointsLength() * 2);
+ for (let kk = 0; kk < lineFb.pointsLength(); ++kk) {
+ points[kk * 2] = lineFb.points(kk).x();
+ points[kk * 2 + 1] = lineFb.points(kk).y();
+ }
+ if (lineFb.color()) {
+ line.setColor(
+ [lineFb.color().r(), lineFb.color().g(), lineFb.color().b()]);
+ }
+ line.setPoints(points);
+ }
+ }
+ });
+}
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 8fbdb7c..0610f6c 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -23,7 +23,9 @@
import * as configuration from 'org_frc971/aos/configuration_generated';
import * as proxy from 'org_frc971/aos/network/www/proxy';
import {plotImu} from 'org_frc971/frc971/wpilib/imu_plotter';
+import {plotDrivetrain} from 'org_frc971/frc971/control_loops/drivetrain/drivetrain_plotter';
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
+import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
import Connection = proxy.Connection;
import Configuration = configuration.aos.Configuration;
@@ -77,7 +79,9 @@
// presence of certain channels.
const plotIndex = new Map<string, PlotState>([
['Demo', new PlotState(plotDiv, plotDemo)],
- ['IMU', new PlotState(plotDiv, plotImu)]
+ ['IMU', new PlotState(plotDiv, plotImu)],
+ ['Drivetrain', new PlotState(plotDiv, plotDrivetrain)],
+ ['C++ Plotter', new PlotState(plotDiv, plotData)],
]);
const invalidSelectValue = 'null';
@@ -112,6 +116,9 @@
}
plotIndex.get(plotSelect.value).initialize(conn);
plotIndex.get(plotSelect.value).show();
+ // Set the URL so that if you reload you get back to this plot.
+ window.history.replaceState(
+ null, null, '?plot=' + encodeURIComponent(plotSelect.value));
});
plotSelect.value = getDefaultPlot();
// Force the event to occur once at the start.
diff --git a/frc971/analysis/plotter_config.json b/frc971/analysis/plotter_config.json
new file mode 100644
index 0000000..49266ee
--- /dev/null
+++ b/frc971/analysis/plotter_config.json
@@ -0,0 +1,12 @@
+{
+ "channels": [
+ {
+ "name": "/analysis",
+ "type": "frc971.analysis.Plot",
+ "max_size": 10000000
+ }
+ ],
+ "imports": [
+ "../../aos/events/aos.json"
+ ]
+}
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 042c8d8..d42b97c 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -3,6 +3,7 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:select.bzl", "cpu_select")
+load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
name = "drivetrain_goal_fbs",
@@ -715,3 +716,14 @@
"//aos/testing:googletest",
],
)
+
+ts_library(
+ name = "drivetrain_plotter",
+ srcs = ["drivetrain_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:proxy",
+ "//frc971/wpilib:imu_plot_utils",
+ ],
+)
diff --git a/frc971/control_loops/drivetrain/camera.h b/frc971/control_loops/drivetrain/camera.h
index 02e7e65..e1d4b76 100644
--- a/frc971/control_loops/drivetrain/camera.h
+++ b/frc971/control_loops/drivetrain/camera.h
@@ -150,14 +150,14 @@
struct Reading {
// The heading as reported from the camera; zero = straight ahead,
// positive = target in the left half of the image.
- Scalar heading; // radians
+ Scalar heading; // radians
// The distance from the camera to the target.
Scalar distance; // meters
// Height of the target from the camera.
- Scalar height; // meters
+ Scalar height; // meters
// The angle of the target relative to line between the camera and
// the center of the target.
- Scalar skew; // radians
+ Scalar skew; // radians
};
Reading reading;
Reading noise;
@@ -283,14 +283,12 @@
apparent_width / noise_parameters_.max_viewable_distance;
view->noise.distance =
noise_parameters_.nominal_distance_noise / normalized_width;
- view->noise.skew =
- noise_parameters_.nominal_skew_noise / normalized_width;
+ view->noise.skew = noise_parameters_.nominal_skew_noise / normalized_width;
view->noise.height =
noise_parameters_.nominal_height_noise / normalized_width;
}
private:
-
// If the specified target is visible from the current camera Pose, adds it to
// the views array.
void AddTargetIfVisible(
@@ -316,7 +314,7 @@
// such a logical obstacle (e.g., the cargo ship) may consist of many
// obstacles in this list to account for all of its sides.
::std::array<LineSegment, num_obstacles> obstacles_;
-}; // class TypedCamera
+}; // class TypedCamera
template <int num_targets, int num_obstacles, typename Scalar>
void TypedCamera<num_targets, num_obstacles, Scalar>::AddTargetIfVisible(
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index cdb7fb9..113eb27 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -92,8 +92,9 @@
const double squared_norm = dspline_point.squaredNorm();
return ddspline_point / squared_norm -
- dspline_point * (dspline_point(0) * ddspline_point(0) +
- dspline_point(1) * ddspline_point(1)) /
+ dspline_point *
+ (dspline_point(0) * ddspline_point(0) +
+ dspline_point(1) * ddspline_point(1)) /
::std::pow(squared_norm, 2);
}
@@ -111,10 +112,10 @@
const double squared_norm = dspline_point.squaredNorm();
- return ddtheta / squared_norm -
- dtheta * (dspline_point(0) * ddspline_point(0) +
- dspline_point(1) * ddspline_point(1)) /
- ::std::pow(squared_norm, 2);
+ return ddtheta / squared_norm - dtheta *
+ (dspline_point(0) * ddspline_point(0) +
+ dspline_point(1) * ddspline_point(1)) /
+ ::std::pow(squared_norm, 2);
}
DistanceSpline::AlphaAndIndex DistanceSpline::DistanceToAlpha(
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index 55b40dd..8189fe9 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -61,10 +61,10 @@
idx_plot.push_back(dpoint(0));
idy_plot.push_back(dpoint(1));
- EXPECT_LT((point - expected_point).norm(), 1e-2) << ": At distance "
- << distance;
- EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2) << ": At distance "
- << distance;
+ EXPECT_LT((point - expected_point).norm(), 1e-2)
+ << ": At distance " << distance;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2)
+ << ": At distance " << distance;
// We need to record the starting state without integrating.
if (i == 0) {
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 13fee1a..5f2d3c4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -1,7 +1,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include <stdio.h>
#include <sched.h>
+#include <stdio.h>
#include <cmath>
#include <memory>
#include "Eigen/Dense"
@@ -114,7 +114,8 @@
down_estimator_.Reset();
// Just reset the localizer to the current state, except for the encoders.
LocalizerInterface::Ekf::State X_hat = localizer_->Xhat();
- X_hat(LocalizerInterface::StateIdx::kLeftEncoder) = position->left_encoder();
+ X_hat(LocalizerInterface::StateIdx::kLeftEncoder) =
+ position->left_encoder();
X_hat(LocalizerInterface::StateIdx::kRightEncoder) =
position->right_encoder();
localizer_->Reset(monotonic_now, X_hat);
@@ -264,9 +265,8 @@
VLOG(1) << "localizer_control "
<< aos::FlatbufferToJson(localizer_control_fetcher_.get());
localizer_->ResetPosition(
- monotonic_now,
- localizer_control_fetcher_->x(), localizer_control_fetcher_->y(),
- localizer_control_fetcher_->theta(),
+ monotonic_now, localizer_control_fetcher_->x(),
+ localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
localizer_control_fetcher_->theta_uncertainty(),
!localizer_control_fetcher_->keep_current_theta());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 3f553bb..b5990c3 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -25,12 +25,12 @@
};
enum class GyroType : int32_t {
- SPARTAN_GYRO = 0, // Use the gyro on the spartan board.
- IMU_X_GYRO = 1, // Use the x-axis of the gyro on the IMU.
- IMU_Y_GYRO = 2, // Use the y-axis of the gyro on the IMU.
- IMU_Z_GYRO = 3, // Use the z-axis of the gyro on the IMU.
- FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
- FLIPPED_IMU_Z_GYRO = 5, // Use the flipped z-axis of the gyro on the IMU.
+ SPARTAN_GYRO = 0, // Use the gyro on the spartan board.
+ IMU_X_GYRO = 1, // Use the x-axis of the gyro on the IMU.
+ IMU_Y_GYRO = 2, // Use the y-axis of the gyro on the IMU.
+ IMU_Z_GYRO = 3, // Use the z-axis of the gyro on the IMU.
+ FLIPPED_SPARTAN_GYRO = 4, // Use the gyro on the spartan board.
+ FLIPPED_IMU_Z_GYRO = 5, // Use the flipped z-axis of the gyro on the IMU.
};
enum class IMUType : int32_t {
@@ -128,7 +128,8 @@
Eigen::Matrix<Scalar, 2, 2> Tlr_to_la() const {
return (::Eigen::Matrix<Scalar, 2, 2>() << 0.5, 0.5,
- -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius)).finished();
+ -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius))
+ .finished();
}
Eigen::Matrix<Scalar, 2, 2> Tla_to_lr() const {
@@ -140,8 +141,7 @@
Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
const Eigen::Matrix<Scalar, 2, 1> &linear,
const Eigen::Matrix<Scalar, 2, 1> &angular) const {
- Eigen::Matrix<Scalar, 2, 1> scaled_angle =
- angular * this->robot_radius;
+ Eigen::Matrix<Scalar, 2, 1> scaled_angle = angular * this->robot_radius;
Eigen::Matrix<Scalar, 4, 1> state;
state(0, 0) = linear(0, 0) - scaled_angle(0, 0);
state(1, 0) = linear(1, 0) - scaled_angle(1, 0);
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
new file mode 100644
index 0000000..6538ec8
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -0,0 +1,360 @@
+// Provides a plot for debugging drivetrain-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import {ImuMessageHandler} from 'org_frc971/frc971/wpilib/imu_plot_utils';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+
+const kRed = [1, 0, 0];
+const kGreen = [0, 1, 0];
+const kBlue = [0, 0, 1];
+const kBrown = [0.6, 0.3, 0];
+const kPink = [1, 0.3, 1];
+const kCyan = [0.3, 1, 1];
+const kWhite = [1, 1, 1];
+
+export function plotDrivetrain(conn: Connection, element: Element): void {
+ const width = 900;
+ const height = 400;
+ const aosPlotter = new AosPlotter(conn);
+
+ const joystickState = aosPlotter.addMessageSource('/aos', 'aos.JoystickState');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+ const goal = aosPlotter.addMessageSource('/drivetrain', 'frc971.control_loops.drivetrain.Goal');
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ const output = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const imu = aosPlotter.addRawMessageSource(
+ '/drivetrain', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ let currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const robotStatePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
+ currentTop += height / 2;
+ robotStatePlot.plot.getAxisLabels().setTitle('Robot State');
+ robotStatePlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ robotStatePlot.plot.getAxisLabels().setYLabel('bool');
+ robotStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ const testMode = robotStatePlot.addMessageLine(joystickState, ['test_mode']);
+ testMode.setColor(kBlue);
+ testMode.setPointSize(0.0);
+ const autoMode = robotStatePlot.addMessageLine(joystickState, ['autonomous']);
+ autoMode.setColor(kRed);
+ autoMode.setPointSize(0.0);
+
+ const brownOut = robotStatePlot.addMessageLine(robotState, ['browned_out']);
+ brownOut.setColor(kBrown);
+ brownOut.setDrawLine(false);
+ const enabled = robotStatePlot.addMessageLine(joystickState, ['enabled']);
+ enabled.setColor(kGreen);
+ enabled.setDrawLine(false);
+
+ // Battery Voltage
+ const batteryPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
+ currentTop += height / 2;
+ batteryPlot.plot.getAxisLabels().setTitle('Battery Voltage');
+ batteryPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ batteryPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
+
+ batteryPlot.addMessageLine(robotState, ['voltage_battery']);
+
+ // Polydrivetrain (teleop control) plots
+ const teleopPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
+ currentTop += height / 2;
+ teleopPlot.plot.getAxisLabels().setTitle('Drivetrain Teleop Goals');
+ teleopPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ teleopPlot.plot.getAxisLabels().setYLabel('bool, throttle/wheel values');
+ teleopPlot.plot.setDefaultYRange([-1.1, 1.1]);
+
+ const quickTurn = teleopPlot.addMessageLine(goal, ['quickturn']);
+ quickTurn.setColor(kRed);
+ const throttle = teleopPlot.addMessageLine(goal, ['throttle']);
+ throttle.setColor(kGreen);
+ const wheel = teleopPlot.addMessageLine(goal, ['wheel']);
+ wheel.setColor(kBlue);
+
+ // Drivetrain Control Mode
+ const modePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
+ currentTop += height / 2;
+ // TODO(james): Actually add enum support.
+ modePlot.plot.getAxisLabels().setTitle(
+ 'Drivetrain Mode [POLYDRIVE, MOTION_PROFILE, ' +
+ 'SPLINE_FOLLOWER, LINE_FOLLOWER]');
+ modePlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ modePlot.plot.getAxisLabels().setYLabel('ControllerType');
+ modePlot.plot.setDefaultYRange([-0.1, 3.1]);
+
+ const controllerType = modePlot.addMessageLine(goal, ['controller_type']);
+ controllerType.setDrawLine(false);
+
+ // Drivetrain Output Voltage
+ const outputPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ outputPlot.plot.getAxisLabels().setTitle('Drivetrain Output');
+ outputPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ outputPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
+
+ const leftVoltage = outputPlot.addMessageLine(output, ['left_voltage']);
+ leftVoltage.setColor(kRed);
+ const rightVoltage = outputPlot.addMessageLine(output, ['right_voltage']);
+ rightVoltage.setColor(kGreen);
+
+ // Voltage Errors
+ const voltageErrors =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ voltageErrors.plot.getAxisLabels().setTitle('Voltage Errors');
+ voltageErrors.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ voltageErrors.plot.getAxisLabels().setYLabel('Voltage (V)');
+
+ const leftVoltageError =
+ voltageErrors.addMessageLine(status, ['left_voltage_error']);
+ leftVoltageError.setColor(kRed);
+ const rightVoltageError =
+ voltageErrors.addMessageLine(status, ['right_voltage_error']);
+ rightVoltageError.setColor(kGreen);
+
+ const ekfLeftVoltageError =
+ voltageErrors.addMessageLine(status, ['localizer', 'left_voltage_error']);
+ ekfLeftVoltageError.setColor(kPink);
+ const ekfRightVoltageError = voltageErrors.addMessageLine(
+ status, ['localizer', 'right_voltage_error']);
+ ekfRightVoltageError.setColor(kCyan);
+
+ // Sundry components of the output voltages
+ const otherVoltages =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ otherVoltages.plot.getAxisLabels().setTitle('Other Voltage Components');
+ otherVoltages.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ otherVoltages.plot.getAxisLabels().setYLabel('Voltage (V)');
+
+ const ffLeftVoltage = otherVoltages.addMessageLine(
+ status, ['poly_drive_logging', 'ff_left_voltage']);
+ ffLeftVoltage.setColor(kRed);
+ ffLeftVoltage.setPointSize(0);
+ const ffRightVoltage = otherVoltages.addMessageLine(
+ status, ['poly_drive_logging', 'ff_right_voltage']);
+ ffRightVoltage.setColor(kGreen);
+ ffRightVoltage.setPointSize(0);
+
+ const uncappedLeftVoltage =
+ otherVoltages.addMessageLine(status, ['uncapped_left_voltage']);
+ uncappedLeftVoltage.setColor(kRed);
+ uncappedLeftVoltage.setDrawLine(false);
+ const uncappedRightVoltage =
+ otherVoltages.addMessageLine(status, ['uncapped_right_voltage']);
+ uncappedRightVoltage.setColor(kGreen);
+ uncappedRightVoltage.setDrawLine(false);
+
+ // Drivetrain Velocities
+ const velocityPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
+ velocityPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
+
+ const ssLeftVelocityGoal =
+ velocityPlot.addMessageLine(status, ['profiled_left_velocity_goal']);
+ ssLeftVelocityGoal.setColor(kPink);
+ ssLeftVelocityGoal.setPointSize(0.0);
+ const ssRightVelocityGoal =
+ velocityPlot.addMessageLine(status, ['profiled_right_velocity_goal']);
+ ssRightVelocityGoal.setColor(kCyan);
+ ssRightVelocityGoal.setPointSize(0.0);
+
+ const polyLeftVelocity = velocityPlot.addMessageLine(
+ status, ['poly_drive_logging', 'goal_left_velocity']);
+ polyLeftVelocity.setColor(kPink);
+ polyLeftVelocity.setDrawLine(false);
+
+ const polyRightVelocity = velocityPlot.addMessageLine(
+ status, ['poly_drive_logging', 'goal_right_velocity']);
+ polyRightVelocity.setColor(kCyan);
+ polyRightVelocity.setDrawLine(false);
+
+ const splineLeftVelocity = velocityPlot.addMessageLine(
+ status, ['trajectory_logging', 'left_velocity']);
+ splineLeftVelocity.setColor(kRed);
+ splineLeftVelocity.setDrawLine(false);
+
+ const splineRightVelocity = velocityPlot.addMessageLine(
+ status, ['trajectory_logging', 'right_velocity']);
+ splineRightVelocity.setColor(kGreen);
+ splineRightVelocity.setDrawLine(false);
+
+ const leftVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
+ leftVelocity.setColor(kRed);
+ const rightVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
+ rightVelocity.setColor(kGreen);
+
+ const ekfLeftVelocity =
+ velocityPlot.addMessageLine(status, ['localizer', 'left_velocity']);
+ ekfLeftVelocity.setColor(kRed);
+ ekfLeftVelocity.setPointSize(0.0);
+ const ekfRightVelocity =
+ velocityPlot.addMessageLine(status, ['localizer', 'right_velocity']);
+ ekfRightVelocity.setColor(kGreen);
+ ekfRightVelocity.setPointSize(0.0);
+
+ // Heading
+ const yawPlot = aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
+ yawPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
+
+ const splineYaw =
+ yawPlot.addMessageLine(status, ['trajectory_logging', 'theta']);
+ splineYaw.setDrawLine(false);
+ splineYaw.setColor(kRed);
+
+ const ekfYaw = yawPlot.addMessageLine(status, ['localizer', 'theta']);
+ ekfYaw.setColor(kGreen);
+
+ const downEstimatorYaw =
+ yawPlot.addMessageLine(status, ['down_estimator', 'yaw']);
+ downEstimatorYaw.setColor(kBlue);
+
+ // Pitch/Roll
+ const orientationPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ orientationPlot.plot.getAxisLabels().setTitle('Orientation');
+ orientationPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
+
+ const roll = orientationPlot.addMessageLine(
+ status, ['down_estimator', 'lateral_pitch']);
+ roll.setColor(kRed);
+ roll.setLabel('roll');
+ const pitch = orientationPlot.addMessageLine(
+ status, ['down_estimator', 'longitudinal_pitch']);
+ pitch.setColor(kGreen);
+ pitch.setLabel('pitch');
+
+ // Accelerometer/Gravity
+ const accelPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ accelPlot.plot.getAxisLabels().setTitle('Accelerometer Readings');
+ accelPlot.plot.getAxisLabels().setYLabel('Acceleration (g)');
+ accelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const expectedAccelX =
+ accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_x']);
+ expectedAccelX.setColor(kRed);
+ expectedAccelX.setPointSize(0);
+ const expectedAccelY =
+ accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_y']);
+ expectedAccelY.setColor(kGreen);
+ expectedAccelY.setPointSize(0);
+ const expectedAccelZ =
+ accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_z']);
+ expectedAccelZ.setColor(kBlue);
+ expectedAccelZ.setPointSize(0);
+
+ const gravity_magnitude =
+ accelPlot.addMessageLine(status, ['down_estimator', 'gravity_magnitude']);
+ gravity_magnitude.setColor(kWhite);
+ gravity_magnitude.setPointSize(0);
+
+ const accelX = accelPlot.addMessageLine(imu, ['accelerometer_x']);
+ accelX.setColor(kRed);
+ accelX.setDrawLine(false);
+ const accelY = accelPlot.addMessageLine(imu, ['accelerometer_y']);
+ accelY.setColor(kGreen);
+ accelY.setDrawLine(false);
+ const accelZ = accelPlot.addMessageLine(imu, ['accelerometer_z']);
+ accelZ.setColor(kBlue);
+ accelZ.setDrawLine(false);
+
+ // Absolute X Position
+ const xPositionPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ xPositionPlot.plot.getAxisLabels().setTitle('X Position');
+ xPositionPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
+
+ const localizerX = xPositionPlot.addMessageLine(status, ['x']);
+ localizerX.setColor(kRed);
+ const splineX =
+ xPositionPlot.addMessageLine(status, ['trajectory_logging', 'x']);
+ splineX.setColor(kGreen);
+
+ // Absolute Y Position
+ const yPositionPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
+ yPositionPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
+
+ const localizerY = yPositionPlot.addMessageLine(status, ['y']);
+ localizerY.setColor(kRed);
+ const splineY =
+ yPositionPlot.addMessageLine(status, ['trajectory_logging', 'y']);
+ splineY.setColor(kGreen);
+
+ // Gyro
+ const gyroPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height]);
+ currentTop += height;
+ gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
+ gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
+ gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const gyroZeroX =
+ gyroPlot.addMessageLine(status, ['zeroing', 'gyro_x_average']);
+ gyroZeroX.setColor(kRed);
+ gyroZeroX.setPointSize(0);
+ gyroZeroX.setLabel('Gyro X Zero');
+ const gyroZeroY =
+ gyroPlot.addMessageLine(status, ['zeroing', 'gyro_y_average']);
+ gyroZeroY.setColor(kGreen);
+ gyroZeroY.setPointSize(0);
+ gyroZeroY.setLabel('Gyro Y Zero');
+ const gyroZeroZ =
+ gyroPlot.addMessageLine(status, ['zeroing', 'gyro_z_average']);
+ gyroZeroZ.setColor(kBlue);
+ gyroZeroZ.setPointSize(0);
+ gyroZeroZ.setLabel('Gyro Z Zero');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor(kRed);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor(kGreen);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor(kBlue);
+
+ // IMU States
+ const imuStatePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
+ currentTop += height / 2;
+ imuStatePlot.plot.getAxisLabels().setTitle('IMU State');
+ imuStatePlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ imuStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ const zeroedLine = imuStatePlot.addMessageLine(status, ['zeroing', 'zeroed']);
+ zeroedLine.setColor(kRed);
+ zeroedLine.setDrawLine(false);
+ zeroedLine.setLabel('IMU Zeroed');
+ const faultedLine =
+ imuStatePlot.addMessageLine(status, ['zeroing', 'faulted']);
+ faultedLine.setColor(kGreen);
+ faultedLine.setPointSize(0);
+ faultedLine.setLabel('IMU Faulted');
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index c7ef9fd..4ff71d6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -108,7 +108,8 @@
drivetrain_status_fetcher_(
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- imu_sender_(event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
+ imu_sender_(
+ event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -267,12 +268,11 @@
U(1, 0) += drivetrain_plant_.right_voltage_offset();
drivetrain_plant_.Update(U);
double dt_float = ::aos::time::DurationInSeconds(dt_config_.dt);
- const auto dynamics =
- [this](const ::Eigen::Matrix<double, 5, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- return ContinuousDynamics(velocity_drivetrain_->plant(),
- dt_config_.Tlr_to_la(), X, U);
- };
+ const auto dynamics = [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(velocity_drivetrain_->plant(),
+ dt_config_.Tlr_to_la(), X, U);
+ };
const Eigen::Matrix<double, 5, 1> last_state = state_;
state_ = RungeKuttaU(dynamics, state_, U, dt_float);
// Calculate Xdot from the actual state change rather than getting Xdot at the
@@ -284,8 +284,7 @@
const Eigen::Matrix<double, 5, 1> Xdot = (state_ - last_state) / dt_float;
const double yaw_rate = Xdot(2);
- const double longitudinal_velocity =
- (state_(4) + state_(3)) / 2.0;
+ const double longitudinal_velocity = (state_(4) + state_(3)) / 2.0;
const double centripetal_accel = yaw_rate * longitudinal_velocity;
// TODO(james): Allow inputting arbitrary calibrations, e.g., for testing
// situations where the IMU is not perfectly flat in the CG of the robot.
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 800b758..e443ba1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -68,7 +68,7 @@
Eigen::Matrix<double, 5, 1> *mutable_state() { return &state_; }
::Eigen::Matrix<double, 2, 1> GetPosition() const {
- return state_.block<2,1>(0,0);
+ return state_.block<2, 1>(0, 0);
}
void MaybePlot();
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
index 4101993..5f8bce4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
@@ -38,10 +38,10 @@
left_goal = 0.0f;
right_goal = 0.0f;
max_ss_voltage = 0.0f;
- //linear.max_velocity = 0.0f;
- //linear.max_acceleration = 0.0f;
- //angular.max_velocity = 0.0f;
- //angular.max_acceleration = 0.0f;
+ // linear.max_velocity = 0.0f;
+ // linear.max_acceleration = 0.0f;
+ // angular.max_velocity = 0.0f;
+ // angular.max_acceleration = 0.0f;
}
DrivetrainQueue_Goal::DrivetrainQueue_Goal() { Zero(); }
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index de4f721..b2abaf9 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -231,14 +231,15 @@
// matrix for linear cases?
void Correct(
const Output &z, const Input *U,
- std::function<
- void(const State &, const StateSquare &,
- std::function<Output(const State &, const Input &)> *,
- std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h,
+ std::function<void(const State &, const StateSquare &,
+ std::function<Output(const State &, const Input &)> *,
+ std::function<Eigen::Matrix<
+ Scalar, kNOutputs, kNStates>(const State &)> *)>
+ make_h,
std::function<Output(const State &, const Input &)> h,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ dhdx,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t);
// A utility function for specifically updating with encoder and gyro
@@ -289,12 +290,11 @@
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
- Correct(
- z, &U, {},
- [this](const State &X, const Input &) {
- return H_encoders_and_gyro_ * X;
- },
- [this](const State &) { return H_encoders_and_gyro_; }, R, t);
+ Correct(z, &U, {},
+ [this](const State &X, const Input &) {
+ return H_encoders_and_gyro_ * X;
+ },
+ [this](const State &) { return H_encoders_and_gyro_; }, R, t);
}
// Sundry accessor:
@@ -359,7 +359,8 @@
std::function<void(const State &, const StateSquare &,
std::function<Output(const State &, const Input &)> *,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h;
+ const State &)> *)>
+ make_h;
// A function to calculate the expected output at a given state/input.
// TODO(james): For encoders/gyro, it is linear and the function call may
// be expensive. Potential source of optimization.
@@ -434,8 +435,7 @@
const Scalar diameter = 2.0 * dt_config_.robot_radius;
const Scalar yaw_rate = CalcYawRate(X);
// X and Y derivatives
- A_continuous(kX, kTheta) =
- -stheta * lng_vel - ctheta * lat_vel;
+ A_continuous(kX, kTheta) = -stheta * lng_vel - ctheta * lat_vel;
A_continuous(kX, kLeftVelocity) = ctheta / 2.0;
A_continuous(kX, kRightVelocity) = ctheta / 2.0;
A_continuous(kX, kLateralVelocity) = -stheta;
@@ -494,8 +494,7 @@
const Scalar expected_lat_accel = lng_vel * yaw_rate;
const Scalar expected_lng_accel =
CalcLongitudinalVelocity(Xdot) - yaw_rate * lat_vel;
- const Scalar lng_accel_offset =
- U(kLongitudinalAccel) - expected_lng_accel;
+ const Scalar lng_accel_offset = U(kLongitudinalAccel) - expected_lng_accel;
constexpr double kAccelWeight = 1.0;
if (!ignore_accel) {
Xdot(kLeftVelocity) += kAccelWeight * lng_accel_offset;
@@ -590,7 +589,6 @@
aos::PriorityQueue<Observation, kSaveSamples, std::less<Observation>>
observations_;
-
friend class testing::HybridEkfTest;
friend class y2019::control_loops::testing::ParameterizedLocalizerTest;
}; // class HybridEkf
@@ -601,10 +599,12 @@
std::function<void(const State &, const StateSquare &,
std::function<Output(const State &, const Input &)> *,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)> make_h,
+ const State &)> *)>
+ make_h,
std::function<Output(const State &, const Input &)> h,
std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ dhdx,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t) {
CHECK(!observations_.empty());
if (!observations_.full() && t < observations_.begin()->t) {
@@ -752,9 +752,9 @@
// noise on these numbers is particularly high, then we can end up with weird
// dynamics where a camera update both shifts our X/Y position and adjusts our
// velocity estimates substantially, causing the camera updates to create
- // "momentum" and if we don't trust the encoders enough, then we have no way of
- // determining that the velocity updates are bogus. This also interacts with
- // kVelocityOffsetTimeConstant.
+ // "momentum" and if we don't trust the encoders enough, then we have no way
+ // of determining that the velocity updates are bogus. This also interacts
+ // with kVelocityOffsetTimeConstant.
Q_continuous_(kLongitudinalVelocityOffset, kLongitudinalVelocityOffset) =
std::pow(1.1, 2.0);
Q_continuous_(kLateralVelocity, kLateralVelocity) = std::pow(0.1, 2.0);
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 48b8b5f..9d9517d 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -76,8 +76,7 @@
EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kLeftVelocity), expected_accel(0));
EXPECT_FLOAT_EQ(Xdot_ekf(StateIdx::kRightVelocity), expected_accel(1));
EXPECT_EQ(Xdot_ekf(StateIdx::kLateralVelocity), 0.0);
- EXPECT_EQ(
- Xdot_ekf(StateIdx::kLongitudinalVelocityOffset), 0.0);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLongitudinalVelocityOffset), 0.0);
} else {
const double lng_accel = U(InputIdx::kLongitudinalAccel) +
lat_vel * yaw_rate + ekf_.VelocityAccel(lng_vel);
@@ -121,7 +120,6 @@
std::normal_distribution<> normal_;
};
-
// Tests that if we provide a bunch of observations of the position
// with zero change in time, the state should approach the estimation.
struct DiffEqInputs {
@@ -152,7 +150,7 @@
const State Xdot_plus = DiffEq(GetParam().X + perturbation, GetParam().U,
GetParam().ignore_accel);
const State Xdot_minus = DiffEq(GetParam().X - perturbation, GetParam().U,
- GetParam().ignore_accel);
+ GetParam().ignore_accel);
const State numerical_dXdot_dX = (Xdot_plus - Xdot_minus) / (2.0 * kEps);
const State A_based_dXdot_dX = A * perturbation / kEps;
EXPECT_LT((A_based_dXdot_dX - numerical_dXdot_dX).norm(), 1e-8)
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 8d2592b..56e7536 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -45,8 +45,8 @@
// States are X_hat_bar (position estimate) and P (Covariance)
void QuaternionUkf::DoPredict(const Eigen::Matrix<double, 3, 1> &U,
- const Eigen::Matrix<double, 3, 1> &measurement,
- const aos::monotonic_clock::duration dt) {
+ const Eigen::Matrix<double, 3, 1> &measurement,
+ const aos::monotonic_clock::duration dt) {
// Compute the sigma points.
// Our system is pretty linear. The traditional way of dealing with process
// noise is to augment your state vector with the mean of the process noise,
@@ -155,8 +155,7 @@
}
// Compute the kalman gain.
- const Eigen::Matrix<double, 3, kNumMeasurements> K =
- P_xz * P_vv.inverse();
+ const Eigen::Matrix<double, 3, kNumMeasurements> K = P_xz * P_vv.inverse();
// Update X_hat and the covariance P
X_hat_ = X_hat_ * Eigen::Quaternion<double>(
@@ -288,8 +287,7 @@
const Eigen::Vector3d robot_y_in_global_frame =
X_hat() * Eigen::Vector3d::UnitY();
const double xy_norm = robot_y_in_global_frame.block<2, 1>(0, 0).norm();
- builder.add_lateral_pitch(
- std::atan2(robot_y_in_global_frame.z(), xy_norm));
+ builder.add_lateral_pitch(std::atan2(robot_y_in_global_frame.z(), xy_norm));
}
builder.add_position_x(pos_vel()(0, 0));
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 79e9b7f..548dba8 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -255,8 +255,7 @@
// determine that calibration routines would be unnecessary).
Eigen::Quaternion<double> Xquat(X);
Eigen::Matrix<double, 3, 1> gprime =
- Xquat.conjugate() * Eigen::Matrix<double, 3, 1>(0.0, 0.0, 1.0) *
- 1.0;
+ Xquat.conjugate() * Eigen::Matrix<double, 3, 1>(0.0, 0.0, 1.0) * 1.0;
return gprime;
}
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index c4643d9..9894f68 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -108,9 +108,10 @@
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
<< "Expected: " << expected.coeffs()
<< " Got: " << dtukf.X_hat().coeffs();
- EXPECT_NEAR(0.0,
- (Eigen::Vector3d(0.0, 1.0, 0.0) - dtukf.H(dtukf.X_hat().coeffs()))
- .norm(), 1e-10);
+ EXPECT_NEAR(
+ 0.0,
+ (Eigen::Vector3d(0.0, 1.0, 0.0) - dtukf.H(dtukf.X_hat().coeffs())).norm(),
+ 1e-10);
}
// Tests that the euler angles in the status message are correct.
@@ -166,7 +167,6 @@
<< " Got: " << dtukf.X_hat().coeffs();
}
-
// Tests that if the gyro indicates no movement but that the accelerometer shows
// that we are slightly rotated, that we eventually adjust our estimate to be
// correct.
@@ -193,9 +193,9 @@
<< " Got: " << dtukf.X_hat().coeffs();
}
-// Tests that if the accelerometer is reading values with a magnitude that isn't ~1g,
-// that we are slightly rotated, that we eventually adjust our estimate to be
-// correct.
+// Tests that if the accelerometer is reading values with a magnitude that isn't
+// ~1g, that we are slightly rotated, that we eventually adjust our estimate to
+// be correct.
TEST(DownEstimatorTest, UkfIgnoreBadAccel) {
drivetrain::DrivetrainUkf dtukf(
drivetrain::testing::GetTestDrivetrainConfig());
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 280171f..6ea23c6 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -1,5 +1,5 @@
-#include <vector>
#include <string>
+#include <vector>
#include "Eigen/Dense"
@@ -15,198 +15,192 @@
namespace drivetrain {
extern "C" {
- // Based on spline.h
- NSpline<6> *NewSpline(double x[6], double y[6]) {
- return new NSpline<6>((::Eigen::Matrix<double, 2, 6>() << x[0], x[1], x[2],
- x[3], x[4], x[5], y[0], y[1], y[2], y[3], y[4],
- y[5]).finished());
+// Based on spline.h
+NSpline<6> *NewSpline(double x[6], double y[6]) {
+ return new NSpline<6>((::Eigen::Matrix<double, 2, 6>() << x[0], x[1], x[2],
+ x[3], x[4], x[5], y[0], y[1], y[2], y[3], y[4], y[5])
+ .finished());
+}
+
+void deleteSpline(NSpline<6> *spline) { delete spline; }
+
+void SplinePoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d xy = spline->Point(alpha);
+ res[0] = xy.x();
+ res[1] = xy.y();
+}
+
+void SplineDPoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d dxy = spline->DPoint(alpha);
+ res[0] = dxy.x();
+ res[1] = dxy.y();
+}
+
+void SplineDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d ddxy = spline->DDPoint(alpha);
+ res[0] = ddxy.x();
+ res[1] = ddxy.y();
+}
+
+void SplineDDDPoint(NSpline<6> *spline, double alpha, double *res) {
+ const Eigen::Vector2d dddxy = spline->DDDPoint(alpha);
+ res[0] = dddxy.x();
+ res[1] = dddxy.y();
+}
+
+double SplineTheta(NSpline<6> *spline, double alpha) {
+ return spline->Theta(alpha);
+}
+
+double SplineDTheta(NSpline<6> *spline, double alpha) {
+ return spline->DTheta(alpha);
+}
+
+double SplineDDTheta(NSpline<6> *spline, double alpha) {
+ return spline->DDTheta(alpha);
+}
+
+void SplineControlPoints(NSpline<6> *spline, double *x, double *y) {
+ auto points = spline->control_points();
+ // Deal with incorrectly strided matrix.
+ for (int i = 0; i < 6; ++i) {
+ x[i] = points(0, i);
+ y[i] = points(1, i);
}
+}
- void deleteSpline(NSpline<6> *spline) { delete spline; }
-
- void SplinePoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d xy = spline->Point(alpha);
- res[0] = xy.x();
- res[1] = xy.y();
+// Based on distance_spline.h
+DistanceSpline *NewDistanceSpline(Spline **splines, int count) {
+ ::std::vector<Spline> splines_;
+ for (int i = 0; i < count; ++i) {
+ splines_.push_back(*splines[i]);
}
+ return new DistanceSpline(::std::vector<Spline>(splines_));
+}
- void SplineDPoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d dxy = spline->DPoint(alpha);
- res[0] = dxy.x();
- res[1] = dxy.y();
+void deleteDistanceSpline(DistanceSpline *spline) { delete spline; }
+
+void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
+ const Eigen::Vector2d xy = spline->XY(distance);
+ res[0] = xy.x();
+ res[1] = xy.y();
+}
+
+void DistanceSplineDXY(DistanceSpline *spline, double distance, double *res) {
+ const Eigen::Vector2d dxy = spline->DXY(distance);
+ res[0] = dxy.x();
+ res[1] = dxy.y();
+}
+
+void DistanceSplineDDXY(DistanceSpline *spline, double distance, double *res) {
+ const Eigen::Vector2d ddxy = spline->DDXY(distance);
+ res[0] = ddxy.x();
+ res[1] = ddxy.y();
+}
+
+double DistanceSplineTheta(DistanceSpline *spline, double distance) {
+ return spline->Theta(distance);
+}
+
+double DistanceSplineDTheta(DistanceSpline *spline, double distance) {
+ return spline->DTheta(distance);
+}
+
+double DistanceSplineDThetaDt(DistanceSpline *spline, double distance,
+ double velocity) {
+ return spline->DThetaDt(distance, velocity);
+}
+
+double DistanceSplineDDTheta(DistanceSpline *spline, double distance) {
+ return spline->DDTheta(distance);
+}
+
+double DistanceSplineLength(DistanceSpline *spline) { return spline->length(); }
+
+// Based on trajectory.h
+Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
+ int num_distance) {
+ return new Trajectory(
+ spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
+ num_distance);
+}
+
+void deleteTrajectory(Trajectory *t) { delete t; }
+
+void TrajectorySetLongitudinalAcceleration(Trajectory *t, double accel) {
+ t->set_longitudinal_acceleration(accel);
+}
+
+void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
+ t->set_lateral_acceleration(accel);
+}
+
+void TrajectorySetVoltageLimit(Trajectory *t, double limit) {
+ t->set_voltage_limit(limit);
+}
+
+void TrajectoryLimitVelocity(Trajectory *t, double start, double end,
+ double max) {
+ t->LimitVelocity(start, end, max);
+}
+
+void TrajectoryPlan(Trajectory *t) { t->Plan(); }
+
+void TrajectoryVoltage(Trajectory *t, double distance, double *res) {
+ const Eigen::Vector2d ff_voltage = t->FFVoltage(distance);
+ res[0] = ff_voltage.x();
+ res[1] = ff_voltage.y();
+}
+
+double TrajectoryLength(Trajectory *t) { return t->length(); }
+
+int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
+
+// This assumes that res is created in python to be getPathLength() long.
+// Likely to SEGFAULT otherwise.
+void TrajectoryDistances(Trajectory *t, double *res) {
+ const ::std::vector<double> &distances = t->Distances();
+ ::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
+}
+
+double TrajectoryDistance(Trajectory *t, int index) {
+ return t->Distance(index);
+}
+
+// This assumes that res is created in python to be getPathLength() long.
+// Likely to SEGFAULT otherwise.
+void TrajectoryGetPlan(Trajectory *t, double *res) {
+ const ::std::vector<double> &velocities = t->plan();
+ ::std::memcpy(res, velocities.data(), sizeof(double) * velocities.size());
+}
+
+// Time in in nanoseconds.
+::std::vector<::Eigen::Matrix<double, 3, 1>> *TrajectoryGetPlanXVAPtr(
+ Trajectory *t, int dt) {
+ return new ::std::vector<::Eigen::Matrix<double, 3, 1>>(
+ t->PlanXVA(::std::chrono::nanoseconds(dt)));
+}
+
+void TrajectoryDeleteVector(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ delete vec;
+}
+
+int TrajectoryGetVectorLength(
+ ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
+ return vec->size();
+}
+
+void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
+ double *X, double *V, double *A) {
+ for (size_t i = 0; i < vec->size(); ++i) {
+ X[i] = (*vec)[i][0];
+ V[i] = (*vec)[i][1];
+ A[i] = (*vec)[i][2];
}
+}
- void SplineDDPoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d ddxy = spline->DDPoint(alpha);
- res[0] = ddxy.x();
- res[1] = ddxy.y();
- }
-
- void SplineDDDPoint(NSpline<6> *spline, double alpha, double *res) {
- const Eigen::Vector2d dddxy = spline->DDDPoint(alpha);
- res[0] = dddxy.x();
- res[1] = dddxy.y();
- }
-
- double SplineTheta(NSpline<6> *spline, double alpha) {
- return spline->Theta(alpha);
- }
-
- double SplineDTheta(NSpline<6> *spline, double alpha) {
- return spline->DTheta(alpha);
- }
-
- double SplineDDTheta(NSpline<6> *spline, double alpha) {
- return spline->DDTheta(alpha);
- }
-
- void SplineControlPoints(NSpline<6> *spline, double *x, double *y) {
- auto points = spline->control_points();
- // Deal with incorrectly strided matrix.
- for (int i = 0; i < 6; ++i) {
- x[i] = points(0, i);
- y[i] = points(1, i);
- }
- }
-
- // Based on distance_spline.h
- DistanceSpline *NewDistanceSpline(Spline **splines, int count) {
- ::std::vector<Spline> splines_;
- for (int i = 0; i < count; ++i) {
- splines_.push_back(*splines[i]);
- }
- return new DistanceSpline(::std::vector<Spline>(splines_));
- }
-
- void deleteDistanceSpline(DistanceSpline *spline) { delete spline; }
-
- void DistanceSplineXY(DistanceSpline *spline, double distance, double *res) {
- const Eigen::Vector2d xy = spline->XY(distance);
- res[0] = xy.x();
- res[1] = xy.y();
- }
-
- void DistanceSplineDXY(DistanceSpline *spline, double distance, double *res) {
- const Eigen::Vector2d dxy = spline->DXY(distance);
- res[0] = dxy.x();
- res[1] = dxy.y();
- }
-
- void DistanceSplineDDXY(DistanceSpline *spline, double distance,
- double *res) {
- const Eigen::Vector2d ddxy = spline->DDXY(distance);
- res[0] = ddxy.x();
- res[1] = ddxy.y();
- }
-
- double DistanceSplineTheta(DistanceSpline *spline, double distance) {
- return spline->Theta(distance);
- }
-
- double DistanceSplineDTheta(DistanceSpline *spline, double distance) {
- return spline->DTheta(distance);
- }
-
- double DistanceSplineDThetaDt(DistanceSpline *spline, double distance,
- double velocity) {
- return spline->DThetaDt(distance, velocity);
- }
-
- double DistanceSplineDDTheta(DistanceSpline *spline, double distance) {
- return spline->DDTheta(distance);
- }
-
- double DistanceSplineLength(DistanceSpline *spline) {
- return spline->length();
- }
-
- // Based on trajectory.h
- Trajectory *NewTrajectory(DistanceSpline *spline, double vmax,
- int num_distance) {
- return new Trajectory(
- spline, ::y2020::control_loops::drivetrain::GetDrivetrainConfig(), vmax,
- num_distance);
- }
-
- void deleteTrajectory(Trajectory *t) { delete t; }
-
- void TrajectorySetLongitudinalAcceleration(Trajectory *t, double accel) {
- t->set_longitudinal_acceleration(accel);
- }
-
- void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
- t->set_lateral_acceleration(accel);
- }
-
- void TrajectorySetVoltageLimit(Trajectory *t, double limit) {
- t->set_voltage_limit(limit);
- }
-
- void TrajectoryLimitVelocity(Trajectory *t, double start, double end,
- double max) {
- t->LimitVelocity(start, end, max);
- }
-
- void TrajectoryPlan(Trajectory *t) { t->Plan(); }
-
- void TrajectoryVoltage(Trajectory *t, double distance, double* res) {
- const Eigen::Vector2d ff_voltage = t->FFVoltage(distance);
- res[0] = ff_voltage.x();
- res[1] = ff_voltage.y();
- }
-
- double TrajectoryLength(Trajectory *t) { return t->length(); }
-
- int TrajectoryGetPathLength(Trajectory *t) { return t->plan().size(); }
-
- // This assumes that res is created in python to be getPathLength() long.
- // Likely to SEGFAULT otherwise.
- void TrajectoryDistances(Trajectory *t, double *res) {
- const ::std::vector<double> &distances = t->Distances();
- ::std::memcpy(res, distances.data(), sizeof(double) * distances.size());
- }
-
- double TrajectoryDistance(Trajectory *t, int index) {
- return t->Distance(index);
- }
-
- // This assumes that res is created in python to be getPathLength() long.
- // Likely to SEGFAULT otherwise.
- void TrajectoryGetPlan(Trajectory *t, double *res) {
- const ::std::vector<double> &velocities = t->plan();
- ::std::memcpy(res, velocities.data(), sizeof(double) * velocities.size());
- }
-
- // Time in in nanoseconds.
- ::std::vector<::Eigen::Matrix<double, 3, 1>> *TrajectoryGetPlanXVAPtr(
- Trajectory *t, int dt) {
- return new ::std::vector<::Eigen::Matrix<double, 3, 1>>(
- t->PlanXVA(::std::chrono::nanoseconds(dt)));
- }
-
- void TrajectoryDeleteVector(
- ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
- delete vec;
- }
-
- int TrajectoryGetVectorLength(
- ::std::vector<::Eigen::Matrix<double, 3, 1>> *vec) {
- return vec->size();
- }
-
- void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
- double *X, double *V, double *A) {
- for (size_t i = 0; i < vec->size(); ++i) {
- X[i] = (*vec)[i][0];
- V[i] = (*vec)[i][1];
- A[i] = (*vec)[i][2];
- }
- }
-
- // Util
- void SetUpLogging() {
- ::aos::network::OverrideTeamNumber(971);
- }
+// Util
+void SetUpLogging() { ::aos::network::OverrideTeamNumber(971); }
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 1e2c76f..a53538b 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -35,11 +35,11 @@
const DrivetrainConfig<double> &dt_config) {
::Eigen::Matrix<double, 3, 2> result;
result.setZero();
- result.block<2, 2>(1, 0) = dt_config.Tlr_to_la() *
- dt_config.make_hybrid_drivetrain_velocity_loop()
- .plant()
- .coefficients()
- .B_continuous;
+ result.block<2, 2>(1, 0) =
+ dt_config.Tlr_to_la() * dt_config.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()
+ .B_continuous;
return result;
}
void AB(const DrivetrainConfig<double> &dt_config,
@@ -64,11 +64,10 @@
return BDiscrete;
}
-::Eigen::Matrix<double, 2, 3> CalcK(
- const ::Eigen::Matrix<double, 3, 3> & A,
- const ::Eigen::Matrix<double, 3, 2> & B,
- const ::Eigen::Matrix<double, 3, 3> & Q,
- const ::Eigen::Matrix<double, 2, 2> & R) {
+::Eigen::Matrix<double, 2, 3> CalcK(const ::Eigen::Matrix<double, 3, 3> &A,
+ const ::Eigen::Matrix<double, 3, 2> &B,
+ const ::Eigen::Matrix<double, 3, 3> &Q,
+ const ::Eigen::Matrix<double, 2, 2> &R) {
Eigen::Matrix<double, 2, 3> K;
Eigen::Matrix<double, 3, 3> S;
int info = ::frc971::controls::dlqr<3, 2>(A, B, Q, R, &K, &S);
@@ -229,8 +228,9 @@
const ::Eigen::Matrix<double, 5, 1> rel_state =
(::Eigen::Matrix<double, 5, 1>() << relative_x,
relative_pose_.rel_pos().y(), relative_pose_.rel_theta(),
- abs_state(3, 0), abs_state(4, 0)).finished();
- if (velocity_sign_ * goal_velocity_ < 0) {
+ abs_state(3, 0), abs_state(4, 0))
+ .finished();
+ if (velocity_sign_ * goal_velocity_ < 0) {
goal_theta = rel_state(2, 0);
}
controls_goal_ << goal_theta, goal_velocity_, 0.0;
@@ -269,7 +269,8 @@
line_follow_logging_builder.add_y(target_pose_.abs_pos().y());
line_follow_logging_builder.add_theta(target_pose_.abs_theta());
line_follow_logging_builder.add_offset(relative_pose_.rel_pos().y());
- line_follow_logging_builder.add_distance_to_target(-relative_pose_.rel_pos().x());
+ line_follow_logging_builder.add_distance_to_target(
+ -relative_pose_.rel_pos().x());
line_follow_logging_builder.add_goal_theta(controls_goal_(0, 0));
line_follow_logging_builder.add_rel_theta(relative_pose_.rel_theta());
return line_follow_logging_builder.Finish();
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 1ebf64d..24f8cd4 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -41,8 +41,7 @@
// Returns whether we set the output. If false, that implies that we do not
// yet have a target to track into and so some other drivetrain should take
// over.
- bool SetOutput(
- ::frc971::control_loops::drivetrain::OutputT *output);
+ bool SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
flatbuffers::Offset<LineFollowLogging> PopulateStatus(
aos::Sender<drivetrain::Status>::Builder *line_follow_logging_builder)
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index a0d374e..b52b67f 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -2,8 +2,6 @@
namespace frc971 {
namespace control_loops {
-namespace drivetrain {
-
-} // namespace drivetrain
+namespace drivetrain {} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/spline_test.cc b/frc971/control_loops/drivetrain/spline_test.cc
index 3ba856d..4fa2001 100644
--- a/frc971/control_loops/drivetrain/spline_test.cc
+++ b/frc971/control_loops/drivetrain/spline_test.cc
@@ -50,7 +50,8 @@
const double alpha =
1.0 * static_cast<double>(i) / static_cast<double>(num_points - 1);
const ::Eigen::Matrix<double, 2, 1> expected_point = spline6_.Point(alpha);
- const ::Eigen::Matrix<double, 2, 1> expected_dpoint = spline6_.DPoint(alpha);
+ const ::Eigen::Matrix<double, 2, 1> expected_dpoint =
+ spline6_.DPoint(alpha);
const ::Eigen::Matrix<double, 2, 1> expected_ddpoint =
spline6_.DDPoint(alpha);
@@ -65,10 +66,10 @@
idy_plot.push_back(dpoint(1));
EXPECT_LT((point - expected_point).norm(), 1e-2) << ": At alpha " << alpha;
- EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2) << ": At alpha "
- << alpha;
- EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-2) << ": At alpha "
- << alpha;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-2)
+ << ": At alpha " << alpha;
+ EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-2)
+ << ": At alpha " << alpha;
// We need to record the starting state without integrating.
if (i == 0) {
@@ -182,12 +183,12 @@
y_plot.push_back(point(1));
EXPECT_LT((point - expected_point).norm(), 1e-9) << ": At alpha " << alpha;
- EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-9) << ": At alpha "
- << alpha;
- EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-9) << ": At alpha "
- << alpha;
- EXPECT_LT((dddpoint - expected_dddpoint).norm(), 1e-9) << ": At alpha "
- << alpha;
+ EXPECT_LT((dpoint - expected_dpoint).norm(), 1e-9)
+ << ": At alpha " << alpha;
+ EXPECT_LT((ddpoint - expected_ddpoint).norm(), 1e-9)
+ << ": At alpha " << alpha;
+ EXPECT_LT((dddpoint - expected_dddpoint).norm(), 1e-9)
+ << ": At alpha " << alpha;
}
// Conditionally plot the functions and their integrals to aid debugging.
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 05d0d9e..423465a 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -183,8 +183,7 @@
enable_ = enable;
if (enable && current_trajectory_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- if (!IsAtEnd() &&
- current_spline_handle_ == current_spline_idx_) {
+ if (!IsAtEnd() && current_spline_handle_ == current_spline_idx_) {
has_started_execution_ = true;
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
@@ -213,7 +212,7 @@
state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
- ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2,1>(0,0);
+ ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2, 1>(0, 0);
next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &xv_state);
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
@@ -237,9 +236,8 @@
output->right_voltage = next_U_(1);
}
-
void SplineDrivetrain::PopulateStatus(
- drivetrain::Status::Builder *builder) const {
+ drivetrain::Status::Builder *builder) const {
if (builder && enable_) {
builder->add_uncapped_left_voltage(uncapped_U_(0));
builder->add_uncapped_right_voltage(uncapped_U_(1));
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 145060d..2ceaac6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -45,22 +45,20 @@
aos::Sender<drivetrain::Status>::Builder *builder) const;
flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
flatbuffers::FlatBufferBuilder *builder) const;
- void PopulateStatus(
- drivetrain::Status::Builder *status) const;
+ void PopulateStatus(drivetrain::Status::Builder *status) const;
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
- return current_trajectory_
- ? current_trajectory_->GoalState(current_xva_(0),
- current_xva_(1))
- : ::Eigen::Matrix<double, 5, 1>::Zero();
+ return current_trajectory_ ? current_trajectory_->GoalState(current_xva_(0),
+ current_xva_(1))
+ : ::Eigen::Matrix<double, 5, 1>::Zero();
}
bool IsAtEnd() const {
return current_trajectory_
- ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0)) :
- true;
+ ? current_trajectory_->is_at_end(current_xva_.block<2, 1>(0, 0))
+ : true;
}
// Returns true if the splinedrivetrain is enabled.
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 12a6bcc..82c83d2 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -113,8 +113,8 @@
bool is_inside_h, is_inside_45;
const auto adjusted_pos_error_h =
DoCoerceGoal<double>(pos_poly_hv, LH, wh, drive_error, &is_inside_h);
- const auto adjusted_pos_error_45 =
- DoCoerceGoal<double>(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+ const auto adjusted_pos_error_45 = DoCoerceGoal<double>(
+ pos_poly_hv, L45, w45, intersection, &is_inside_45);
if (pos_poly_hv.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
@@ -131,8 +131,8 @@
}
}
- *U = -U_integral + velocity_K *velocity_error +
- position_K *T_ *adjusted_pos_error + kf_->ff_U();
+ *U = -U_integral + velocity_K * velocity_error +
+ position_K * T_ * adjusted_pos_error + kf_->ff_U();
if (!output_was_capped_) {
if ((*U - kf_->U_uncapped()).norm() > 0.0001) {
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 729415c..ff60b4c 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -45,7 +45,7 @@
void Trajectory::LateralAccelPass() {
for (size_t i = 0; i < plan_.size(); ++i) {
const double distance = Distance(i);
- const double velocity_limit = LateralVelocityCurvature(distance);
+ const double velocity_limit = LateralVelocityCurvature(distance);
if (velocity_limit < plan_[i]) {
plan_[i] = velocity_limit;
plan_segment_type_[i] = CURVATURE_LIMITED;
@@ -102,8 +102,8 @@
}
if (best_accel < min_voltage_accel || best_accel > max_voltage_accel) {
LOG(WARNING) << "Viable friction limits and viable voltage limits do not "
- "overlap (x: " << x << ", v: " << v
- << ", backwards: " << backwards
+ "overlap (x: "
+ << x << ", v: " << v << ", backwards: " << backwards
<< ") best_accel = " << best_accel << ", min voltage "
<< min_voltage_accel << ", max voltage " << max_voltage_accel
<< " min friction " << min_friction_accel << " max friction "
@@ -414,7 +414,7 @@
// by the acceleration/deceleration limits. This may not always be true;
// if we ever encounter this error, we just need to back out what the
// accelerations would be in this case.
- LOG(FATAL) << "Unexpectedly got VOLTAGE_LIMITED plan.";
+ LOG(FATAL) << "Unexpectedly got VOLTAGE_LIMITED plan.";
break;
case SegmentType::ACCELERATION_LIMITED:
// TODO(james): The integration done here and in the DECELERATION_LIMITED
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 0801541..ef02db5 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -225,7 +225,8 @@
const ::Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const {
return (::Eigen::Matrix<double, 2, 1>()
<< -robot_radius_l_ * current_ddtheta,
- robot_radius_r_ * current_ddtheta).finished();
+ robot_radius_r_ * current_ddtheta)
+ .finished();
}
const ::Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const {
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index f301e9e..2770f63 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -67,16 +67,14 @@
::std::unique_ptr<Trajectory> trajectory_;
::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
- ParameterizedSplineTest()
- : dt_config_(GetTestDrivetrainConfig()) {}
+ ParameterizedSplineTest() : dt_config_(GetTestDrivetrainConfig()) {}
void SetUp() {
distance_spline_ = ::std::unique_ptr<DistanceSpline>(
new DistanceSpline(Spline(GetParam().control_points)));
// Run lots of steps to make the feedforwards terms more accurate.
- trajectory_ = ::std::unique_ptr<Trajectory>(
- new Trajectory(distance_spline_.get(), dt_config_,
- GetParam().velocity_limit));
+ trajectory_ = ::std::unique_ptr<Trajectory>(new Trajectory(
+ distance_spline_.get(), dt_config_, GetParam().velocity_limit));
trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
trajectory_->set_longitudinal_acceleration(
GetParam().longitudinal_acceleration);
@@ -85,8 +83,7 @@
GetParam().trajectory_modification_fn(trajectory_.get());
initial_plan_ = trajectory_->plan();
- trajectory_->VoltageFeasibilityPass(
- Trajectory::VoltageLimit::kAggressive);
+ trajectory_->VoltageFeasibilityPass(Trajectory::VoltageLimit::kAggressive);
aggressive_voltage_plan_ = trajectory_->plan();
trajectory_->VoltageFeasibilityPass(
Trajectory::VoltageLimit::kConservative);
@@ -136,7 +133,8 @@
matplotlibcpp::plot(distances, backward_plan_, {{"label", "backward"}});
matplotlibcpp::plot(distances, forward_plan_, {{"label", "forward"}});
matplotlibcpp::plot(distances, curvature_plan_, {{"label", "lateral"}});
- matplotlibcpp::plot(distances, aggressive_voltage_plan_, {{"label", "aggressive voltage"}});
+ matplotlibcpp::plot(distances, aggressive_voltage_plan_,
+ {{"label", "aggressive voltage"}});
matplotlibcpp::plot(distances, voltage_plan_, {{"label", "voltage"}});
matplotlibcpp::plot(distances, initial_plan_, {{"label", "initial"}});
matplotlibcpp::legend();
@@ -187,7 +185,6 @@
::std::vector<double> length_plan_a_;
::std::vector<double> length_plan_vl_;
::std::vector<double> length_plan_vr_;
-
};
// Tests that the VoltageVelocityLimit function produces correct results by
@@ -206,7 +203,7 @@
const ::Eigen::Matrix<double, 2, 2> B =
trajectory_->velocity_drivetrain().plant().coefficients().B_continuous;
const double conservative_limit = trajectory_->VoltageVelocityLimit(
- distance, Trajectory::VoltageLimit::kConservative, &U);
+ distance, Trajectory::VoltageLimit::kConservative, &U);
ASSERT_LT(0.0, conservative_limit)
<< "Voltage limit should be strictly positive.";
const bool on_straight_line = distance_spline_->DTheta(distance) == 0 &&
@@ -245,8 +242,7 @@
const double aggressive_limit = trajectory_->VoltageVelocityLimit(
distance, Trajectory::VoltageLimit::kAggressive, &U);
if (on_straight_line) {
- EXPECT_EQ(::std::numeric_limits<double>::infinity(),
- aggressive_limit);
+ EXPECT_EQ(::std::numeric_limits<double>::infinity(), aggressive_limit);
continue;
}
EXPECT_TRUE((GetParam().voltage_limit == U.array().abs()).all()) << U;
@@ -264,14 +260,12 @@
const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
K2 * perturbed_accel + K1 * aggressive_limit * aggressive_limit;
const ::Eigen::Matrix<double, 2, 1> perturbed_voltages =
- B.inverse() *
- (perturbed_wheel_accels - A * K2 * aggressive_limit);
+ B.inverse() * (perturbed_wheel_accels - A * K2 * aggressive_limit);
EXPECT_GT(perturbed_voltages.lpNorm<::Eigen::Infinity>(),
GetParam().voltage_limit)
<< "We were able to perturb the voltage!!.";
}
}
-
}
}
@@ -446,7 +440,7 @@
Eigen::Matrix<double, 5, 5> A_continuous;
Eigen::Matrix<double, 5, 2> B_continuous;
trajectory_->PathRelativeContinuousSystem(distance, &A_continuous,
- &B_continuous);
+ &B_continuous);
Eigen::Matrix<double, 5, 5> A_discrete;
Eigen::Matrix<double, 5, 2> B_discrete;
controls::C2D(A_continuous, B_continuous, dt_config_.dt, &A_discrete,
@@ -541,7 +535,8 @@
trajectory->LimitVelocity(1.0, 2.0, 0.5);
}
-void ShortLimitMiddleOfPathTrajectoryModificationFunction(Trajectory *trajectory) {
+void ShortLimitMiddleOfPathTrajectoryModificationFunction(
+ Trajectory *trajectory) {
trajectory->LimitVelocity(1.5, 1.5, 0.5);
}