Merge "Set the slope to 0 before the start in NoncausalTimestampFilter"
diff --git a/.bazelrc b/.bazelrc
index 9a26282..fce2616 100644
--- a/.bazelrc
+++ b/.bazelrc
@@ -26,6 +26,9 @@
build:armhf-debian --platforms=//tools/platforms:linux_armhf
build:cortex-m4f --platforms=//tools/platforms:cortex_m4f
+# Without this, we end up rebuilding from scratch every time we change compilers. This is needed to make --cpu work (even though it shouldn't be used).
+build --crosstool_top=@//tools/cpp:toolchain --host_crosstool_top=@//tools/cpp:toolchain
+
build:asan --copt -fsanitize=address
build:asan --linkopt -fsanitize=address --linkopt -ldl
build:asan --platform_suffix=-asan
diff --git a/aos/events/logging/log_cat.cc b/aos/events/logging/log_cat.cc
index f9da914..c8407f1 100644
--- a/aos/events/logging/log_cat.cc
+++ b/aos/events/logging/log_cat.cc
@@ -147,7 +147,8 @@
bool found_channel = false;
- for (const aos::Node *node : reader.Nodes()) {
+ for (const aos::Node *node :
+ aos::configuration::GetNodes(event_loop_factory.configuration())) {
std::unique_ptr<aos::EventLoop> printer_event_loop =
event_loop_factory.MakeEventLoop("printer", node);
printer_event_loop->SkipTimingReport();
diff --git a/aos/events/logging/log_stats.cc b/aos/events/logging/log_stats.cc
index a2d8184..47d0ced 100644
--- a/aos/events/logging/log_stats.cc
+++ b/aos/events/logging/log_stats.cc
@@ -83,7 +83,7 @@
if (aos::configuration::MultiNode(reader.configuration())) {
if (FLAGS_node.empty()) {
LOG(INFO) << "Need a --node specified. The log file has:";
- for (const aos::Node *node : reader.Nodes()) {
+ for (const aos::Node *node : reader.LoggedNodes()) {
LOG(INFO) << " " << node->name()->string_view();
}
reader.Deregister();
diff --git a/aos/events/logging/logger.cc b/aos/events/logging/logger.cc
index 493f09a..8c6d936 100644
--- a/aos/events/logging/logger.cc
+++ b/aos/events/logging/logger.cc
@@ -1070,16 +1070,8 @@
return remapped_configuration_;
}
-std::vector<const Node *> LogReader::Nodes() const {
- // Because the Node pointer will only be valid if it actually points to
- // memory owned by remapped_configuration_, we need to wait for the
- // remapped_configuration_ to be populated before accessing it.
- //
- // Also, note, that when ever a map is changed, the nodes in here are
- // invalidated.
- CHECK(remapped_configuration_ != nullptr)
- << ": Need to call Register before the node() pointer will be valid.";
- return configuration::GetNodes(remapped_configuration_);
+std::vector<const Node *> LogReader::LoggedNodes() const {
+ return configuration::GetNodes(logged_configuration());
}
monotonic_clock::time_point LogReader::monotonic_start_time(
diff --git a/aos/events/logging/logger.h b/aos/events/logging/logger.h
index aa6be40..e221695 100644
--- a/aos/events/logging/logger.h
+++ b/aos/events/logging/logger.h
@@ -403,9 +403,8 @@
const Configuration *configuration() const;
// Returns the nodes that this log file was created on. This is a list of
- // pointers to a node in the nodes() list inside configuration(). The
- // pointers here are invalidated whenever RemapLoggedChannel is called.
- std::vector<const Node *> Nodes() const;
+ // pointers to a node in the nodes() list inside logged_configuration().
+ std::vector<const Node *> LoggedNodes() const;
// Returns the starting timestamp for the log file.
monotonic_clock::time_point monotonic_start_time(
diff --git a/aos/events/logging/logger_main.cc b/aos/events/logging/logger_main.cc
index a05c06f..da84cc9 100644
--- a/aos/events/logging/logger_main.cc
+++ b/aos/events/logging/logger_main.cc
@@ -30,14 +30,9 @@
aos::ShmEventLoop event_loop(&config.message());
std::unique_ptr<aos::logger::LogNamer> log_namer;
- if (event_loop.node() == nullptr) {
- log_namer = std::make_unique<aos::logger::LocalLogNamer>(
- aos::logging::GetLogName("fbs_log"), event_loop.node());
- } else {
- log_namer = std::make_unique<aos::logger::MultiNodeLogNamer>(
- absl::StrCat(aos::logging::GetLogName("fbs_log"), "/"),
- event_loop.configuration(), event_loop.node());
- }
+ log_namer = std::make_unique<aos::logger::MultiNodeLogNamer>(
+ absl::StrCat(aos::logging::GetLogName("fbs_log"), "/"),
+ event_loop.configuration(), event_loop.node());
aos::logger::Logger logger(&event_loop);
event_loop.OnRun([&log_namer, &logger]() {
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index a28891e..5f18e29 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -96,7 +96,7 @@
// log file.
reader.Register();
- EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(nullptr));
+ EXPECT_THAT(reader.LoggedNodes(), ::testing::ElementsAre(nullptr));
std::unique_ptr<EventLoop> test_event_loop =
reader.event_loop_factory()->MakeEventLoop("log_reader");
@@ -237,7 +237,7 @@
LogReader reader(std::get<0>(logfile));
reader.Register();
- EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(nullptr));
+ EXPECT_THAT(reader.LoggedNodes(), ::testing::ElementsAre(nullptr));
std::unique_ptr<EventLoop> test_event_loop =
reader.event_loop_factory()->MakeEventLoop("log_reader");
@@ -322,7 +322,7 @@
// log file.
reader.Register();
- EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(nullptr));
+ EXPECT_THAT(reader.LoggedNodes(), ::testing::ElementsAre(nullptr));
std::unique_ptr<EventLoop> test_event_loop =
reader.event_loop_factory()->MakeEventLoop("log_reader");
@@ -943,7 +943,10 @@
LOG(INFO) << "now pi2 "
<< log_reader_factory.GetNodeEventLoopFactory(pi2)->monotonic_now();
- EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(pi1, pi2));
+ EXPECT_THAT(reader.LoggedNodes(),
+ ::testing::ElementsAre(
+ configuration::GetNode(reader.logged_configuration(), pi1),
+ configuration::GetNode(reader.logged_configuration(), pi2)));
reader.event_loop_factory()->set_send_delay(chrono::microseconds(0));
@@ -1137,7 +1140,10 @@
const Node *pi2 =
configuration::GetNode(log_reader_factory.configuration(), "pi2");
- EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(pi1, pi2));
+ EXPECT_THAT(reader.LoggedNodes(),
+ ::testing::ElementsAre(
+ configuration::GetNode(reader.logged_configuration(), pi1),
+ configuration::GetNode(reader.logged_configuration(), pi2)));
reader.event_loop_factory()->set_send_delay(chrono::microseconds(0));
@@ -1313,7 +1319,10 @@
<< " "
<< log_reader_factory.GetNodeEventLoopFactory(pi2)->realtime_now();
- EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(pi1, pi2));
+ EXPECT_THAT(reader.LoggedNodes(),
+ ::testing::ElementsAre(
+ configuration::GetNode(reader.logged_configuration(), pi1),
+ configuration::GetNode(reader.logged_configuration(), pi2)));
reader.event_loop_factory()->set_send_delay(chrono::microseconds(0));
@@ -1594,7 +1603,10 @@
LOG(INFO) << "now pi2 "
<< log_reader_factory.GetNodeEventLoopFactory(pi2)->monotonic_now();
- EXPECT_THAT(reader.Nodes(), ::testing::ElementsAre(pi1, pi2));
+ EXPECT_THAT(reader.LoggedNodes(),
+ ::testing::ElementsAre(
+ configuration::GetNode(reader.logged_configuration(), pi1),
+ configuration::GetNode(reader.logged_configuration(), pi2)));
reader.event_loop_factory()->set_send_delay(chrono::microseconds(0));
diff --git a/aos/network/www/aos_plotter.ts b/aos/network/www/aos_plotter.ts
index 33a3de1..ada69a9 100644
--- a/aos/network/www/aos_plotter.ts
+++ b/aos/network/www/aos_plotter.ts
@@ -131,6 +131,21 @@
}
export class AosPlotter {
+
+ public static readonly TIME: string = "Monotonic Time (sec)";
+
+ public static readonly DEFAULT_WIDTH: number = 900;
+ public static readonly DEFAULT_HEIGHT: number = 400;
+
+ public static readonly RED: number[] = [1, 0, 0];
+ public static readonly GREEN: number[] = [0, 1, 0];
+ public static readonly BLUE: number[] = [0, 0, 1];
+ public static readonly BROWN: number[] = [0.6, 0.3, 0];
+ public static readonly PINK: number[] = [1, 0.3, 0.6];
+ public static readonly CYAN: number[] = [0.3, 1, 1];
+ public static readonly WHITE: number[] = [1, 1, 1];
+
+
private plots: AosPlot[] = [];
private messages = new Set<MessageHandler>();
constructor(private readonly connection: Connection) {
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index 8ad3dbf..de6e249 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -96,6 +96,7 @@
deps = [
":starter_rpc_lib",
"@com_github_google_glog//:glog",
+ "@com_google_absl//absl/strings:str_format",
],
)
diff --git a/aos/starter/starter_cmd.cc b/aos/starter/starter_cmd.cc
index d076afc..1381fa3 100644
--- a/aos/starter/starter_cmd.cc
+++ b/aos/starter/starter_cmd.cc
@@ -1,7 +1,9 @@
#include <chrono>
+#include <functional>
#include <iostream>
#include <unordered_map>
+#include "absl/strings/str_format.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
#include "gflags/gflags.h"
@@ -9,36 +11,53 @@
DEFINE_string(config, "./config.json", "File path of aos configuration");
-static const std::unordered_map<std::string, aos::starter::Command> kCommands{
- {"start", aos::starter::Command::START},
- {"stop", aos::starter::Command::STOP},
- {"restart", aos::starter::Command::RESTART}};
+namespace {
-int main(int argc, char **argv) {
- aos::InitGoogle(&argc, &argv);
+static const std::unordered_map<std::string, aos::starter::Command>
+ kCommandConversions{{"start", aos::starter::Command::START},
+ {"stop", aos::starter::Command::STOP},
+ {"restart", aos::starter::Command::RESTART}};
- CHECK(argc == 3) << "Invalid number of command arguments";
-
- const std::string application_name = argv[1];
- const std::string command_str = argv[2];
-
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
-
- if (command_str == "status") {
- auto status = aos::starter::GetStatus(application_name, &config.message());
+bool GetStarterStatus(int argc, char **argv, const aos::Configuration *config) {
+ if (argc == 1) {
+ // Print status for all processes.
+ auto status = aos::starter::GetStarterStatus(config);
+ for (const aos::starter::ApplicationStatus *app_status :
+ *status.message().statuses()) {
+ absl::PrintF("%-30s %s\n", app_status->name()->string_view(),
+ aos::starter::EnumNameState(app_status->state()));
+ }
+ } else if (argc == 2) {
+ // Print status for the specified process.
+ const char *application_name = argv[1];
+ auto status = aos::starter::GetStatus(application_name, config);
std::cout << aos::FlatbufferToJson(&status.message()) << '\n';
+ } else {
+ LOG(ERROR) << "The \"status\" command requires zero or one arguments.";
+ return true;
+ }
+ return false;
+}
- return 0;
+bool InteractWithProgram(int argc, char **argv,
+ const aos::Configuration *config) {
+ const char *command_string = argv[0];
+
+ if (argc != 2) {
+ LOG(ERROR) << "The \"" << command_string
+ << "\" command requires an application name as an argument.";
+ return true;
}
- const auto command_search = kCommands.find(command_str);
- CHECK(command_search != kCommands.end())
- << "Invalid command \"" << command_str << "\"";
- const aos::starter::Command command = command_search->second;
+ const auto command_search = kCommandConversions.find(command_string);
+ CHECK(command_search != kCommandConversions.end())
+ << "Internal error: \"" << command_string
+ << "\" is not in kCommandConversions.";
- if (aos::starter::SendCommandBlocking(command, application_name,
- &config.message(),
+ const aos::starter::Command command = command_search->second;
+ const char *application_name = argv[1];
+
+ if (aos::starter::SendCommandBlocking(command, application_name, config,
std::chrono::seconds(3))) {
switch (command) {
case aos::starter::Command::START:
@@ -52,6 +71,57 @@
break;
}
} else {
- std::cout << "Failed to " << command_str << ' ' << application_name << '\n';
+ std::cout << "Failed to " << command_string << ' ' << application_name
+ << '\n';
}
+ return false;
+}
+
+// This is the set of subcommands we support. Each subcommand accepts argc and
+// argv from its own point of view. So argv[0] is always the name of the
+// subcommand. argv[1] and up are the arguments to the subcommand.
+// The subcommand returns true if there was an error parsing the command line
+// arguments. It returns false when the command line arguments are parsed
+// successfully.
+static const std::unordered_map<
+ std::string, std::function<bool(int argc, char **argv,
+ const aos::Configuration *config)>>
+ kCommands{
+ {"status", GetStarterStatus},
+ {"start", InteractWithProgram},
+ {"stop", InteractWithProgram},
+ {"restart", InteractWithProgram},
+ };
+
+} // namespace
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ bool parsing_failed = false;
+
+ if (argc < 2) {
+ parsing_failed = true;
+ } else {
+ const char *command = argv[1];
+ auto it = kCommands.find(command);
+ if (it == kCommands.end()) {
+ parsing_failed = true;
+ } else {
+ parsing_failed = it->second(argc - 1, argv + 1, &config.message());
+ }
+ }
+
+ if (parsing_failed) {
+ LOG(ERROR) << "Parsing failed. Valid commands are:";
+ for (auto entry: kCommands) {
+ LOG(ERROR) << " - " << entry.first;
+ }
+ return 1;
+ }
+
+ return 0;
}
diff --git a/aos/starter/starter_rpc_lib.cc b/aos/starter/starter_rpc_lib.cc
index efb4042..89fc2c2 100644
--- a/aos/starter/starter_rpc_lib.cc
+++ b/aos/starter/starter_rpc_lib.cc
@@ -127,5 +127,19 @@
aos::starter::ApplicationStatus>::Empty();
}
+const aos::FlatbufferVector<aos::starter::Status> GetStarterStatus(
+ const aos::Configuration *config) {
+ ShmEventLoop event_loop(config);
+ event_loop.SkipAosLog();
+
+ auto status_fetcher = event_loop.MakeFetcher<aos::starter::Status>("/aos");
+ status_fetcher.Fetch();
+ if (status_fetcher) {
+ return status_fetcher.CopyFlatBuffer();
+ } else {
+ return FlatbufferVector<aos::starter::Status>::Empty();
+ }
+}
+
} // namespace starter
} // namespace aos
diff --git a/aos/starter/starter_rpc_lib.h b/aos/starter/starter_rpc_lib.h
index 57c9e6b..152016a 100644
--- a/aos/starter/starter_rpc_lib.h
+++ b/aos/starter/starter_rpc_lib.h
@@ -30,6 +30,11 @@
const aos::FlatbufferDetachedBuffer<aos::starter::ApplicationStatus> GetStatus(
std::string_view name, const aos::Configuration *config);
+// Fetches the entire status message of starter. Creates a temporary event loop
+// from the provided config for fetching.
+const aos::FlatbufferVector<aos::starter::Status> GetStarterStatus(
+ const aos::Configuration *config);
+
} // namespace starter
} // namespace aos
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index f378b68..45e947b 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -96,6 +96,7 @@
"//aos/network/www:demo_plot",
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_plotter",
+ "//frc971/control_loops/drivetrain:robot_state_plotter",
"//frc971/wpilib:imu_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 0610f6c..780f944 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -24,6 +24,8 @@
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 {plotRobotState} from
+ 'org_frc971/frc971/control_loops/drivetrain/robot_state_plotter'
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
@@ -81,6 +83,7 @@
['Demo', new PlotState(plotDiv, plotDemo)],
['IMU', new PlotState(plotDiv, plotImu)],
['Drivetrain', new PlotState(plotDiv, plotDrivetrain)],
+ ['Robot State', new PlotState(plotDiv, plotRobotState)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
]);
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index d42b97c..ad39bcf 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -285,6 +285,7 @@
":drivetrain_config",
":drivetrain_goal_fbs",
":drivetrain_output_fbs",
+ ":drivetrain_states",
":drivetrain_status_fbs",
":gear",
":localizer",
@@ -315,6 +316,7 @@
}),
deps = [
":drivetrain_config",
+ ":drivetrain_states",
":gear",
"//aos:math",
"//aos/controls:polytope",
@@ -368,6 +370,11 @@
)
cc_library(
+ name = "drivetrain_states",
+ hdrs = ["drivetrain_states.h"],
+)
+
+cc_library(
name = "drivetrain_lib",
srcs = [
"drivetrain.cc",
@@ -381,6 +388,7 @@
":drivetrain_goal_fbs",
":drivetrain_output_fbs",
":drivetrain_position_fbs",
+ ":drivetrain_states",
":drivetrain_status_fbs",
":gear",
":improved_down_estimator",
@@ -727,3 +735,13 @@
"//frc971/wpilib:imu_plot_utils",
],
)
+
+ts_library(
+ name = "robot_state_plotter",
+ srcs = ["robot_state_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:proxy",
+ ],
+)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 5f2d3c4..7b7fbca 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -300,8 +300,12 @@
// TODO(james): The regular Kalman Filter's voltage error terms are
// currently unusable--either don't use voltage error at all for the spline
// following code, or use the EKF's voltage error estimates.
+ static_assert(kLeftError + 1 == kRightError);
+ Eigen::Matrix<double, 2, 2> error_K;
+ error_K << kf_.controller().K(kLeftVoltage, kLeftError), 0.0, 0.0,
+ kf_.controller().K(kRightVoltage, kRightError);
const Eigen::Matrix<double, 2, 1> voltage_error =
- 0 * kf_.X_hat().block<2, 1>(4, 0);
+ 0 * error_K * kf_.X_hat().block<2, 1>(kLeftError, 0);
dt_spline_.Update(
output != nullptr && controller_type == ControllerType::SPLINE_FOLLOWER,
trajectory_state, voltage_error);
@@ -369,24 +373,25 @@
dt_closedloop_.PopulateStatus(&builder);
- builder.add_estimated_left_position(gyro_left_right(0, 0));
- builder.add_estimated_right_position(gyro_left_right(2, 0));
+ builder.add_estimated_left_position(gyro_left_right(kLeftPosition));
+ builder.add_estimated_right_position(gyro_left_right(kRightPosition));
- builder.add_estimated_left_velocity(gyro_left_right(1, 0));
- builder.add_estimated_right_velocity(gyro_left_right(3, 0));
+ builder.add_estimated_left_velocity(gyro_left_right(kLeftVelocity));
+ builder.add_estimated_right_velocity(gyro_left_right(kRightVelocity));
if (dt_spline_.enable()) {
dt_spline_.PopulateStatus(&builder);
} else {
- builder.add_robot_speed((kf_.X_hat(1) + kf_.X_hat(3)) / 2.0);
+ builder.add_robot_speed(
+ (kf_.X_hat(kLeftVelocity) + kf_.X_hat(kRightVelocity)) / 2.0);
builder.add_output_was_capped(dt_closedloop_.output_was_capped());
- builder.add_uncapped_left_voltage(kf_.U_uncapped(0, 0));
- builder.add_uncapped_right_voltage(kf_.U_uncapped(1, 0));
+ builder.add_uncapped_left_voltage(kf_.U_uncapped(kLeftVoltage));
+ builder.add_uncapped_right_voltage(kf_.U_uncapped(kRightVoltage));
}
- builder.add_left_voltage_error(kf_.X_hat(4));
- builder.add_right_voltage_error(kf_.X_hat(5));
- builder.add_estimated_angular_velocity_error(kf_.X_hat(6));
+ builder.add_left_voltage_error(kf_.X_hat(kLeftError));
+ builder.add_right_voltage_error(kf_.X_hat(kRightError));
+ builder.add_estimated_angular_velocity_error(kf_.X_hat(kAngularError));
builder.add_estimated_heading(localizer_->theta());
builder.add_x(localizer_->x());
@@ -429,8 +434,8 @@
last_last_left_voltage_ = last_left_voltage_;
last_last_right_voltage_ = last_right_voltage_;
Eigen::Matrix<double, 2, 1> U;
- U(0, 0) = last_left_voltage_;
- U(1, 0) = last_right_voltage_;
+ U(kLeftVoltage) = last_left_voltage_;
+ U(kRightVoltage) = last_right_voltage_;
last_left_voltage_ = left_voltage;
last_right_voltage_ = right_voltage;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index d350321..f282cc0 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_states.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index dd42d20..f1ddd75 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -5,22 +5,23 @@
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];
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+const RED = AosPlotter.RED;
+const GREEN = AosPlotter.GREEN;
+const BLUE = AosPlotter.BLUE;
+const BROWN = AosPlotter.BROWN;
+const PINK = AosPlotter.PINK;
+const CYAN = AosPlotter.CYAN;
+const WHITE = AosPlotter.WHITE;
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 position = aosPlotter.addMessageSource("/drivetrain",
+ "frc971.control_loops.drivetrain.Position");
const status = aosPlotter.addMessageSource(
'/drivetrain', 'frc971.control_loops.drivetrain.Status');
const output = aosPlotter.addMessageSource(
@@ -31,351 +32,322 @@
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;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
teleopPlot.plot.getAxisLabels().setTitle('Drivetrain Teleop Goals');
- teleopPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ teleopPlot.plot.getAxisLabels().setXLabel(TIME);
teleopPlot.plot.getAxisLabels().setYLabel('bool, throttle/wheel values');
teleopPlot.plot.setDefaultYRange([-1.1, 1.1]);
const quickTurn = teleopPlot.addMessageLine(goal, ['quickturn']);
- quickTurn.setColor(kRed);
+ quickTurn.setColor(RED);
const throttle = teleopPlot.addMessageLine(goal, ['throttle']);
- throttle.setColor(kGreen);
+ throttle.setColor(GREEN);
const wheel = teleopPlot.addMessageLine(goal, ['wheel']);
- wheel.setColor(kBlue);
+ wheel.setColor(BLUE);
// Drivetrain Control Mode
const modePlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
- currentTop += height / 2;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_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().setXLabel(TIME);
modePlot.plot.getAxisLabels().setYLabel('ControllerType');
modePlot.plot.setDefaultYRange([-0.1, 3.1]);
const controllerType = modePlot.addMessageLine(goal, ['controller_type']);
controllerType.setDrawLine(false);
- // Drivetrain estimated relative position
+ // Drivetrain Status estimated relative position
const positionPlot = aosPlotter.addPlot(element, [0, currentTop],
- [width, height]);
- currentTop += height;
+ [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
"of the Drivetrain");
- positionPlot.plot.getAxisLabels().setXLabel("Monotonic Time (sec)");
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
const leftPosition =
positionPlot.addMessageLine(status, ["estimated_left_position"]);
- leftPosition.setColor(kRed);
+ leftPosition.setColor(RED);
const rightPosition =
positionPlot.addMessageLine(status, ["estimated_right_position"]);
- rightPosition.setColor(kGreen);
+ rightPosition.setColor(GREEN);
const leftPositionGoal =
positionPlot.addMessageLine(status, ["profiled_left_position_goal"]);
- leftPositionGoal.setColor(kBlue);
+ leftPositionGoal.setColor(BLUE);
const rightPositionGoal =
positionPlot.addMessageLine(status, ["profiled_right_position_goal"]);
- rightPositionGoal.setColor(kPink);
+ rightPositionGoal.setColor(PINK);
+ const leftEncoder = positionPlot.addMessageLine(position, ["left_encoder"]);
+ leftEncoder.setColor(BROWN);
+ const rightEncoder = positionPlot.addMessageLine(position, ["right_encoder"]);
+ rightEncoder.setColor(CYAN);
// Drivetrain Output Voltage
const outputPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
outputPlot.plot.getAxisLabels().setTitle('Drivetrain Output');
- outputPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ outputPlot.plot.getAxisLabels().setXLabel(TIME);
outputPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
const leftVoltage = outputPlot.addMessageLine(output, ['left_voltage']);
- leftVoltage.setColor(kRed);
+ leftVoltage.setColor(RED);
const rightVoltage = outputPlot.addMessageLine(output, ['right_voltage']);
- rightVoltage.setColor(kGreen);
+ rightVoltage.setColor(GREEN);
// Voltage Errors
const voltageErrors =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
voltageErrors.plot.getAxisLabels().setTitle('Voltage Errors');
- voltageErrors.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ voltageErrors.plot.getAxisLabels().setXLabel(TIME);
voltageErrors.plot.getAxisLabels().setYLabel('Voltage (V)');
const leftVoltageError =
voltageErrors.addMessageLine(status, ['left_voltage_error']);
- leftVoltageError.setColor(kRed);
+ leftVoltageError.setColor(RED);
const rightVoltageError =
voltageErrors.addMessageLine(status, ['right_voltage_error']);
- rightVoltageError.setColor(kGreen);
+ rightVoltageError.setColor(GREEN);
const ekfLeftVoltageError =
voltageErrors.addMessageLine(status, ['localizer', 'left_voltage_error']);
- ekfLeftVoltageError.setColor(kPink);
+ ekfLeftVoltageError.setColor(PINK);
const ekfRightVoltageError = voltageErrors.addMessageLine(
status, ['localizer', 'right_voltage_error']);
- ekfRightVoltageError.setColor(kCyan);
+ ekfRightVoltageError.setColor(CYAN);
// Sundry components of the output voltages
const otherVoltages =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
otherVoltages.plot.getAxisLabels().setTitle('Other Voltage Components');
- otherVoltages.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ otherVoltages.plot.getAxisLabels().setXLabel(TIME);
otherVoltages.plot.getAxisLabels().setYLabel('Voltage (V)');
const ffLeftVoltage = otherVoltages.addMessageLine(
status, ['poly_drive_logging', 'ff_left_voltage']);
- ffLeftVoltage.setColor(kRed);
+ ffLeftVoltage.setColor(RED);
ffLeftVoltage.setPointSize(0);
const ffRightVoltage = otherVoltages.addMessageLine(
status, ['poly_drive_logging', 'ff_right_voltage']);
- ffRightVoltage.setColor(kGreen);
+ ffRightVoltage.setColor(GREEN);
ffRightVoltage.setPointSize(0);
const uncappedLeftVoltage =
otherVoltages.addMessageLine(status, ['uncapped_left_voltage']);
- uncappedLeftVoltage.setColor(kRed);
+ uncappedLeftVoltage.setColor(RED);
uncappedLeftVoltage.setDrawLine(false);
const uncappedRightVoltage =
otherVoltages.addMessageLine(status, ['uncapped_right_voltage']);
- uncappedRightVoltage.setColor(kGreen);
+ uncappedRightVoltage.setColor(GREEN);
uncappedRightVoltage.setDrawLine(false);
// Drivetrain Velocities
const velocityPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
- velocityPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
const ssLeftVelocityGoal =
velocityPlot.addMessageLine(status, ['profiled_left_velocity_goal']);
- ssLeftVelocityGoal.setColor(kPink);
+ ssLeftVelocityGoal.setColor(PINK);
ssLeftVelocityGoal.setPointSize(0.0);
const ssRightVelocityGoal =
velocityPlot.addMessageLine(status, ['profiled_right_velocity_goal']);
- ssRightVelocityGoal.setColor(kCyan);
+ ssRightVelocityGoal.setColor(CYAN);
ssRightVelocityGoal.setPointSize(0.0);
const polyLeftVelocity = velocityPlot.addMessageLine(
status, ['poly_drive_logging', 'goal_left_velocity']);
- polyLeftVelocity.setColor(kPink);
+ polyLeftVelocity.setColor(PINK);
polyLeftVelocity.setDrawLine(false);
const polyRightVelocity = velocityPlot.addMessageLine(
status, ['poly_drive_logging', 'goal_right_velocity']);
- polyRightVelocity.setColor(kCyan);
+ polyRightVelocity.setColor(CYAN);
polyRightVelocity.setDrawLine(false);
const splineLeftVelocity = velocityPlot.addMessageLine(
status, ['trajectory_logging', 'left_velocity']);
- splineLeftVelocity.setColor(kRed);
+ splineLeftVelocity.setColor(RED);
splineLeftVelocity.setDrawLine(false);
const splineRightVelocity = velocityPlot.addMessageLine(
status, ['trajectory_logging', 'right_velocity']);
- splineRightVelocity.setColor(kGreen);
+ splineRightVelocity.setColor(GREEN);
splineRightVelocity.setDrawLine(false);
const leftVelocity =
velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
- leftVelocity.setColor(kRed);
+ leftVelocity.setColor(RED);
const rightVelocity =
velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
- rightVelocity.setColor(kGreen);
+ rightVelocity.setColor(GREEN);
const ekfLeftVelocity =
velocityPlot.addMessageLine(status, ['localizer', 'left_velocity']);
- ekfLeftVelocity.setColor(kRed);
+ ekfLeftVelocity.setColor(RED);
ekfLeftVelocity.setPointSize(0.0);
const ekfRightVelocity =
velocityPlot.addMessageLine(status, ['localizer', 'right_velocity']);
- ekfRightVelocity.setColor(kGreen);
+ ekfRightVelocity.setColor(GREEN);
ekfRightVelocity.setPointSize(0.0);
// Heading
- const yawPlot = aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ const yawPlot = aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
- yawPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ yawPlot.plot.getAxisLabels().setXLabel(TIME);
yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
const splineYaw =
yawPlot.addMessageLine(status, ['trajectory_logging', 'theta']);
splineYaw.setDrawLine(false);
- splineYaw.setColor(kRed);
+ splineYaw.setColor(RED);
const ekfYaw = yawPlot.addMessageLine(status, ['localizer', 'theta']);
- ekfYaw.setColor(kGreen);
+ ekfYaw.setColor(GREEN);
const downEstimatorYaw =
yawPlot.addMessageLine(status, ['down_estimator', 'yaw']);
- downEstimatorYaw.setColor(kBlue);
+ downEstimatorYaw.setColor(BLUE);
// Pitch/Roll
const orientationPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
orientationPlot.plot.getAxisLabels().setTitle('Orientation');
- orientationPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ orientationPlot.plot.getAxisLabels().setXLabel(TIME);
orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
const roll = orientationPlot.addMessageLine(
status, ['down_estimator', 'lateral_pitch']);
- roll.setColor(kRed);
+ roll.setColor(RED);
roll.setLabel('roll');
const pitch = orientationPlot.addMessageLine(
status, ['down_estimator', 'longitudinal_pitch']);
- pitch.setColor(kGreen);
+ pitch.setColor(GREEN);
pitch.setLabel('pitch');
// Accelerometer/Gravity
const accelPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_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.setColor(RED);
expectedAccelX.setPointSize(0);
const expectedAccelY =
accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_y']);
- expectedAccelY.setColor(kGreen);
+ expectedAccelY.setColor(GREEN);
expectedAccelY.setPointSize(0);
const expectedAccelZ =
accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_z']);
- expectedAccelZ.setColor(kBlue);
+ expectedAccelZ.setColor(BLUE);
expectedAccelZ.setPointSize(0);
const gravity_magnitude =
accelPlot.addMessageLine(status, ['down_estimator', 'gravity_magnitude']);
- gravity_magnitude.setColor(kWhite);
+ gravity_magnitude.setColor(WHITE);
gravity_magnitude.setPointSize(0);
const accelX = accelPlot.addMessageLine(imu, ['accelerometer_x']);
- accelX.setColor(kRed);
+ accelX.setColor(RED);
accelX.setDrawLine(false);
const accelY = accelPlot.addMessageLine(imu, ['accelerometer_y']);
- accelY.setColor(kGreen);
+ accelY.setColor(GREEN);
accelY.setDrawLine(false);
const accelZ = accelPlot.addMessageLine(imu, ['accelerometer_z']);
- accelZ.setColor(kBlue);
+ accelZ.setColor(BLUE);
accelZ.setDrawLine(false);
// Absolute X Position
const xPositionPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
xPositionPlot.plot.getAxisLabels().setTitle('X Position');
- xPositionPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
const localizerX = xPositionPlot.addMessageLine(status, ['x']);
- localizerX.setColor(kRed);
+ localizerX.setColor(RED);
const splineX =
xPositionPlot.addMessageLine(status, ['trajectory_logging', 'x']);
- splineX.setColor(kGreen);
+ splineX.setColor(GREEN);
// Absolute Y Position
const yPositionPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
- yPositionPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
const localizerY = yPositionPlot.addMessageLine(status, ['y']);
- localizerY.setColor(kRed);
+ localizerY.setColor(RED);
const splineY =
yPositionPlot.addMessageLine(status, ['trajectory_logging', 'y']);
- splineY.setColor(kGreen);
+ splineY.setColor(GREEN);
// Gyro
const gyroPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_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.setColor(RED);
gyroZeroX.setPointSize(0);
gyroZeroX.setLabel('Gyro X Zero');
const gyroZeroY =
gyroPlot.addMessageLine(status, ['zeroing', 'gyro_y_average']);
- gyroZeroY.setColor(kGreen);
+ gyroZeroY.setColor(GREEN);
gyroZeroY.setPointSize(0);
gyroZeroY.setLabel('Gyro Y Zero');
const gyroZeroZ =
gyroPlot.addMessageLine(status, ['zeroing', 'gyro_z_average']);
- gyroZeroZ.setColor(kBlue);
+ gyroZeroZ.setColor(BLUE);
gyroZeroZ.setPointSize(0);
gyroZeroZ.setLabel('Gyro Z Zero');
const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
- gyroX.setColor(kRed);
+ gyroX.setColor(RED);
const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
- gyroY.setColor(kGreen);
+ gyroY.setColor(GREEN);
const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
- gyroZ.setColor(kBlue);
+ gyroZ.setColor(BLUE);
// IMU States
const imuStatePlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
- currentTop += height / 2;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
imuStatePlot.plot.getAxisLabels().setTitle('IMU State');
- imuStatePlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ imuStatePlot.plot.getAxisLabels().setXLabel(TIME);
imuStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
const zeroedLine = imuStatePlot.addMessageLine(status, ['zeroing', 'zeroed']);
- zeroedLine.setColor(kRed);
+ zeroedLine.setColor(RED);
zeroedLine.setDrawLine(false);
zeroedLine.setLabel('IMU Zeroed');
const faultedLine =
imuStatePlot.addMessageLine(status, ['zeroing', 'faulted']);
- faultedLine.setColor(kGreen);
+ faultedLine.setColor(GREEN);
faultedLine.setPointSize(0);
faultedLine.setLabel('IMU Faulted');
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_states.h b/frc971/control_loops/drivetrain/drivetrain_states.h
new file mode 100644
index 0000000..9ea2518
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_states.h
@@ -0,0 +1,24 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_STATES_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_STATES_H_
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+enum KalmanState {
+ kLeftPosition = 0,
+ kLeftVelocity = 1,
+ kRightPosition = 2,
+ kRightVelocity = 3,
+ kLeftError = 4,
+ kRightError = 5,
+ kAngularError = 6
+};
+
+enum OutputState { kLeftVoltage = 0, kRightVoltage = 1 };
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_STATES_H_
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index d6cd2b5..17a32b8 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -20,6 +20,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_float_generated.h"
#endif // __linux__
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_states.h"
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
@@ -294,8 +295,8 @@
template <typename Scalar>
void PolyDrivetrain<Scalar>::Update(Scalar voltage_battery) {
if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
- loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
- loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
+ loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(kLeftVelocity);
+ loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(kRightVelocity);
}
// TODO(austin): Observer for the current velocity instead of difference
diff --git a/frc971/control_loops/drivetrain/robot_state_plotter.ts b/frc971/control_loops/drivetrain/robot_state_plotter.ts
new file mode 100644
index 0000000..f42c50b
--- /dev/null
+++ b/frc971/control_loops/drivetrain/robot_state_plotter.ts
@@ -0,0 +1,66 @@
+// 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 Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+const RED = AosPlotter.RED;
+const GREEN = AosPlotter.GREEN;
+const BLUE = AosPlotter.BLUE;
+const BROWN = AosPlotter.BROWN;
+const PINK = AosPlotter.PINK;
+const CYAN = AosPlotter.CYAN;
+const WHITE = AosPlotter.WHITE;
+
+export function plotRobotState(conn: Connection, element: Element) : void {
+ const aosPlotter = new AosPlotter(conn);
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+ const joystickState = aosPlotter.addMessageSource('/aos', 'aos.JoystickState');
+
+ var currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const robotStatePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ robotStatePlot.plot.getAxisLabels().setTitle('Robot State');
+ robotStatePlot.plot.getAxisLabels().setXLabel(TIME);
+ robotStatePlot.plot.getAxisLabels().setYLabel('bool');
+ robotStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ const testMode = robotStatePlot.addMessageLine(joystickState, ['test_mode']);
+ testMode.setColor(BLUE);
+ testMode.setPointSize(0.0);
+ const autoMode = robotStatePlot.addMessageLine(joystickState, ['autonomous']);
+ autoMode.setColor(RED);
+ autoMode.setPointSize(0.0);
+
+ const brownOut = robotStatePlot.addMessageLine(robotState, ['browned_out']);
+ brownOut.setColor(BROWN);
+ brownOut.setDrawLine(false);
+ const enabled = robotStatePlot.addMessageLine(joystickState, ['enabled']);
+ enabled.setColor(GREEN);
+ enabled.setDrawLine(false);
+
+ // Battery Voltage
+ const batteryPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ batteryPlot.plot.getAxisLabels().setTitle('Battery Voltage');
+ batteryPlot.plot.getAxisLabels().setXLabel(TIME);
+ batteryPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
+
+ batteryPlot.addMessageLine(robotState, ['voltage_battery']);
+
+ // PID of process reading sensors
+ const readerPidPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ readerPidPlot.plot.getAxisLabels().setTitle("PID of Process Reading Sensors");
+ readerPidPlot.plot.getAxisLabels().setXLabel(TIME);
+ readerPidPlot.plot.getAxisLabels().setYLabel("wpilib_interface Process ID");
+ readerPidPlot.addMessageLine(robotState, ["reader_pid"]);
+}
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 82c83d2..3e58d09 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -62,21 +62,30 @@
const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
Eigen::Matrix<double, 2, 2> position_K;
- position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
- kf_->controller().K(1, 0), kf_->controller().K(1, 2);
+ position_K << kf_->controller().K(kLeftVoltage, kLeftPosition),
+ kf_->controller().K(kLeftVoltage, kRightPosition),
+ kf_->controller().K(kRightVoltage, kLeftPosition),
+ kf_->controller().K(kRightVoltage, kRightPosition);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << kf_->controller().K(0, 1), kf_->controller().K(0, 3),
- kf_->controller().K(1, 1), kf_->controller().K(1, 3);
+ velocity_K << kf_->controller().K(kLeftVoltage, kLeftVelocity),
+ kf_->controller().K(kLeftVoltage, kRightVelocity),
+ kf_->controller().K(kRightVoltage, kLeftVelocity),
+ kf_->controller().K(kRightVoltage, kRightVelocity);
+
+ Eigen::Matrix<double, 2, 2> error_K;
+ error_K << kf_->controller().K(kLeftVoltage, kLeftError), 0.0, 0.0,
+ kf_->controller().K(kRightVoltage, kRightError);
Eigen::Matrix<double, 2, 1> position_error;
- position_error << error(0, 0), error(2, 0);
+ position_error << error(kLeftPosition), error(kRightPosition);
// drive_error = [total_distance_error, left_error - right_error]
const auto drive_error = T_inverse_ * position_error;
Eigen::Matrix<double, 2, 1> velocity_error;
- velocity_error << error(1, 0), error(3, 0);
+ velocity_error << error(kLeftVelocity), error(kRightVelocity);
- Eigen::Matrix<double, 2, 1> U_integral;
- U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
+ Eigen::Matrix<double, 2, 1> U_integral =
+ error_K * Eigen::Matrix<double, 2, 1>(kf_->X_hat(kLeftError),
+ kf_->X_hat(kRightError));
const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
U_poly_.static_H() * position_K * T_,
@@ -201,11 +210,11 @@
(gyro_to_wheel_offset - last_gyro_to_wheel_offset_) *
dt_config_.robot_radius;
- kf_->mutable_next_R().block<4, 1>(0, 0) =
+ kf_->mutable_next_R().block<4, 1>(kLeftPosition, 0) =
dt_config_.AngularLinearToLeftRight(next_linear, next_angular);
- kf_->mutable_next_R().block<3, 1>(4, 0) =
- unprofiled_goal_.block<3, 1>(4, 0);
+ kf_->mutable_next_R().block<3, 1>(kLeftError, 0) =
+ unprofiled_goal_.block<3, 1>(kLeftError, 0);
kf_->mutable_next_R(0, 0) -= wheel_compensation_offset;
kf_->mutable_next_R(2, 0) += wheel_compensation_offset;
@@ -263,8 +272,8 @@
void DrivetrainMotorsSS::SetOutput(
::frc971::control_loops::drivetrain::OutputT *output) const {
if (output) {
- output->left_voltage = kf_->U(0, 0);
- output->right_voltage = kf_->U(1, 0);
+ output->left_voltage = kf_->U(kLeftVoltage);
+ output->right_voltage = kf_->U(kRightVoltage);
output->left_high = true;
output->right_high = true;
}
@@ -282,10 +291,14 @@
Eigen::Matrix<double, 4, 1> profiled_gyro_left_right =
dt_config_.AngularLinearToLeftRight(profiled_linear, profiled_angular);
- builder->add_profiled_left_position_goal(profiled_gyro_left_right(0, 0));
- builder->add_profiled_left_velocity_goal(profiled_gyro_left_right(1, 0));
- builder->add_profiled_right_position_goal(profiled_gyro_left_right(2, 0));
- builder->add_profiled_right_velocity_goal(profiled_gyro_left_right(3, 0));
+ builder->add_profiled_left_position_goal(
+ profiled_gyro_left_right(kLeftPosition));
+ builder->add_profiled_left_velocity_goal(
+ profiled_gyro_left_right(kLeftVelocity));
+ builder->add_profiled_right_position_goal(
+ profiled_gyro_left_right(kRightPosition));
+ builder->add_profiled_right_velocity_goal(
+ profiled_gyro_left_right(kRightVelocity));
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 4d91807..d83473d 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_states.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/state_feedback_loop.h"
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index aa29d6b..1aec459 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -46,19 +46,20 @@
G_high: float, Gear ratio for high gear.
G_low: float, Gear ratio for low gear.
dt: float, Control loop time step.
- q_pos: float, q position for a single speed.
- q_pos_low: float, q position low gear.
- q_pos_high: float, q position high gear.
- q_vel: float, q velocity for a single speed
- q_vel_low: float, q velocity low gear.
- q_vel_high: float, q velocity high gear.
+ q_pos: float, q position for a single speed LQR controller.
+ q_pos_low: float, q position low gear LQR controller.
+ q_pos_high: float, q position high gear LQR controller.
+ q_vel: float, q velocity for a single speed LQR controller.
+ q_vel_low: float, q velocity low gear LQR controller.
+ q_vel_high: float, q velocity high gear LQR controller.
efficiency: float, gear box effiency.
has_imu: bool, true if imu is present.
force: bool, true if force.
kf_q_voltage: float
motor_type: object, class of values defining the motor in drivetrain.
num_motors: int, number of motors on one side of drivetrain.
- controller_poles: array, An array of poles. (See control_loop.py)
+ controller_poles: array, An array of poles for the polydrivetrain
+ controller. (See control_loop.py)
observer_poles: array, An array of poles. (See control_loop.py)
robot_cg_offset: offset in meters of CG from robot center to left side
"""
@@ -249,7 +250,12 @@
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, self._name)
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('Poles: %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug('Time constants: %s hz',
+ str([
+ numpy.log(x) / -self.dt
+ for x in numpy.linalg.eig(self.A - self.B * self.K)[0]
+ ]))
glog.debug('K %s', repr(self.K))
self.hlp = 0.3
@@ -526,8 +532,35 @@
def PlotDrivetrainMotions(drivetrain_params):
+ # Test out the voltage error.
+ drivetrain = KFDrivetrain(
+ left_low=False, right_low=False, drivetrain_params=drivetrain_params)
+ close_loop_left = []
+ close_loop_right = []
+ left_power = []
+ right_power = []
+ R = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
+ for _ in xrange(300):
+ U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
+ drivetrain.U_max)
+ drivetrain.UpdateObserver(U)
+ drivetrain.Update(U + numpy.matrix([[1.0], [1.0]]))
+ close_loop_left.append(drivetrain.X[0, 0])
+ close_loop_right.append(drivetrain.X[2, 0])
+ left_power.append(U[0, 0])
+ right_power.append(U[1, 0])
+
+ t = [drivetrain.dt * x for x in range(300)]
+ pylab.plot(t, close_loop_left, label='left position')
+ pylab.plot(t, close_loop_right, 'm--', label='right position')
+ pylab.plot(t, left_power, label='left power')
+ pylab.plot(t, right_power, '--', label='right power')
+ pylab.suptitle('Voltage error')
+ pylab.legend()
+ pylab.show()
+
# Simulate the response of the system to a step input.
- drivetrain = Drivetrain(
+ drivetrain = KFDrivetrain(
left_low=False, right_low=False, drivetrain_params=drivetrain_params)
simulated_left = []
simulated_right = []
@@ -544,13 +577,13 @@
pylab.show()
# Simulate forwards motion.
- drivetrain = Drivetrain(
+ drivetrain = KFDrivetrain(
left_low=False, right_low=False, drivetrain_params=drivetrain_params)
close_loop_left = []
close_loop_right = []
left_power = []
right_power = []
- R = numpy.matrix([[1.0], [0.0], [1.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [1.0], [0.0], [0.0], [0.0], [0.0]])
for _ in xrange(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
@@ -561,19 +594,20 @@
left_power.append(U[0, 0])
right_power.append(U[1, 0])
- pylab.plot(range(300), close_loop_left, label='left position')
- pylab.plot(range(300), close_loop_right, 'm--', label='right position')
- pylab.plot(range(300), left_power, label='left power')
- pylab.plot(range(300), right_power, '--', label='right power')
+ t = [drivetrain.dt * x for x in range(300)]
+ pylab.plot(t, close_loop_left, label='left position')
+ pylab.plot(t, close_loop_right, 'm--', label='right position')
+ pylab.plot(t, left_power, label='left power')
+ pylab.plot(t, right_power, '--', label='right power')
pylab.suptitle('Linear Move\nLeft and Right Position going to 1')
pylab.legend()
pylab.show()
# Try turning in place
- drivetrain = Drivetrain(drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(drivetrain_params=drivetrain_params)
close_loop_left = []
close_loop_right = []
- R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0]])
+ R = numpy.matrix([[-1.0], [0.0], [1.0], [0.0], [0.0], [0.0], [0.0]])
for _ in xrange(200):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
@@ -590,10 +624,10 @@
pylab.show()
# Try turning just one side.
- drivetrain = Drivetrain(drivetrain_params=drivetrain_params)
+ drivetrain = KFDrivetrain(drivetrain_params=drivetrain_params)
close_loop_left = []
close_loop_right = []
- R = numpy.matrix([[0.0], [0.0], [1.0], [0.0]])
+ R = numpy.matrix([[0.0], [0.0], [1.0], [0.0], [0.0], [0.0], [0.0]])
for _ in xrange(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 168df43..50edf0d 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -413,38 +413,38 @@
}
Scalar X_hat(int i, int j = 0) const { return X_hat()(i, j); }
const Eigen::Matrix<Scalar, number_of_states, 1> &R() const { return R_; }
- Scalar R(int i, int j) const { return R()(i, j); }
+ Scalar R(int i, int j = 0) const { return R()(i, j); }
const Eigen::Matrix<Scalar, number_of_states, 1> &next_R() const {
return next_R_;
}
- Scalar next_R(int i, int j) const { return next_R()(i, j); }
+ Scalar next_R(int i, int j = 0) const { return next_R()(i, j); }
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U() const { return U_; }
- Scalar U(int i, int j) const { return U()(i, j); }
+ Scalar U(int i, int j = 0) const { return U()(i, j); }
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_uncapped() const {
return U_uncapped_;
}
- Scalar U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
+ Scalar U_uncapped(int i, int j = 0) const { return U_uncapped()(i, j); }
const Eigen::Matrix<Scalar, number_of_inputs, 1> &ff_U() const {
return ff_U_;
}
- Scalar ff_U(int i, int j) const { return ff_U()(i, j); }
+ Scalar ff_U(int i, int j = 0) const { return ff_U()(i, j); }
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() {
return observer_.mutable_X_hat();
}
Scalar &mutable_X_hat(int i, int j = 0) { return mutable_X_hat()(i, j); }
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_R() { return R_; }
- Scalar &mutable_R(int i, int j) { return mutable_R()(i, j); }
+ Scalar &mutable_R(int i, int j = 0) { return mutable_R()(i, j); }
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_next_R() {
return next_R_;
}
- Scalar &mutable_next_R(int i, int j) { return mutable_next_R()(i, j); }
+ Scalar &mutable_next_R(int i, int j = 0) { return mutable_next_R()(i, j); }
Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U() { return U_; }
- Scalar &mutable_U(int i, int j) { return mutable_U()(i, j); }
+ Scalar &mutable_U(int i, int j = 0) { return mutable_U()(i, j); }
Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U_uncapped() {
return U_uncapped_;
}
- Scalar &mutable_U_uncapped(int i, int j) {
+ Scalar &mutable_U_uncapped(int i, int j = 0) {
return mutable_U_uncapped()(i, j);
}
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
index 9cf9a67..c27638f 100644
--- a/frc971/wpilib/drivetrain_writer.h
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -13,6 +13,8 @@
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
::frc971::control_loops::drivetrain::Output> {
public:
+ static constexpr double kMaxBringupPower = 12.0;
+
DrivetrainWriter(::aos::EventLoop *event_loop)
: ::frc971::wpilib::LoopOutputHandler<
::frc971::control_loops::drivetrain::Output>(event_loop,
@@ -44,7 +46,9 @@
void Stop() override;
double SafeSpeed(bool reversed, double voltage) {
- return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -12.0, 12.0) / 12.0);
+ return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
}
::std::unique_ptr<::frc::PWM> left_controller0_, right_controller0_,
diff --git a/frc971/wpilib/wpilib_robot_base.h b/frc971/wpilib/wpilib_robot_base.h
index e34e3ad..33a17bc 100644
--- a/frc971/wpilib/wpilib_robot_base.h
+++ b/frc971/wpilib/wpilib_robot_base.h
@@ -48,18 +48,19 @@
class WPILibAdapterRobot : public frc::RobotBase {
public:
void StartCompetition() override {
+ PCHECK(setuid(0) == 0) << ": Failed to change user to root";
// Just allow overcommit memory like usual. Various processes map memory
// they will never use, and the roboRIO doesn't have enough RAM to handle
// it. This is in here instead of starter.sh because starter.sh doesn't run
// with permissions on a roboRIO.
- AOS_CHECK(system("echo 0 > /proc/sys/vm/overcommit_memory") == 0);
+ PCHECK(system("echo 0 > /proc/sys/vm/overcommit_memory") == 0);
// Configure throttling so we reserve 5% of the CPU for non-rt work.
// This makes things significantly more stable when work explodes.
// This is in here instead of starter.sh for the same reasons, starter is
// suid and runs as admin, so this actually works.
- AOS_CHECK(system("/sbin/sysctl -w kernel.sched_rt_period_us=1000000") == 0);
- AOS_CHECK(system("/sbin/sysctl -w kernel.sched_rt_runtime_us=950000") == 0);
+ PCHECK(system("/sbin/sysctl -w kernel.sched_rt_period_us=1000000") == 0);
+ PCHECK(system("/sbin/sysctl -w kernel.sched_rt_runtime_us=950000") == 0);
robot_.Run();
}
diff --git a/platform_mappings b/platform_mappings
new file mode 100644
index 0000000..04265dd
--- /dev/null
+++ b/platform_mappings
@@ -0,0 +1,26 @@
+# https://docs.bazel.build/versions/master/platforms-intro.html#platform-mappings
+platforms:
+ //tools/platforms:linux_x86
+ --cpu=k8
+
+ //tools/platforms:linux_roborio
+ --cpu=roborio
+
+ //tools/platforms:linux_armhf
+ --cpu=armhf-debian
+
+ //tools/platforms:cortex_m4f
+ --cpu=cortex-m4f
+
+flags:
+ --cpu=k8
+ //tools/platforms:linux_x86
+
+ --cpu=roborio
+ //tools/platforms:linux_roborio
+
+ --cpu=armhf-debian
+ //tools/platforms:linux_armhf
+
+ --cpu=cortex-m4f
+ //tools/platforms:cortex_m4f
diff --git a/tools/cpp/BUILD b/tools/cpp/BUILD
index 4f0e6d9..ea25dd1 100644
--- a/tools/cpp/BUILD
+++ b/tools/cpp/BUILD
@@ -2,6 +2,17 @@
package(default_visibility = ["//visibility:public"])
+cc_toolchain_suite(
+ name = "toolchain",
+ toolchains = {
+ "k8": ":cc-compiler-k8",
+ "armhf-debian": ":cc-compiler-armhf-debian",
+ "roborio": ":cc-compiler-roborio",
+ "cortex-m4f": ":cc-compiler-cortex-m4f",
+ },
+ visibility = ["//visibility:public"],
+)
+
[
cc_toolchain_config(
name = "{}_toolchain_config".format(cpu),
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 0b2b9c6..9b526dd 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -11,6 +11,8 @@
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/actors/auto_splines.h"
+DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
+
namespace y2020 {
namespace actors {
@@ -26,7 +28,9 @@
::frc971::control_loops::drivetrain::LocalizerControl>(
"/drivetrain")),
joystick_state_fetcher_(
- event_loop->MakeFetcher<aos::JoystickState>("/aos")) {}
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ set_max_drivetrain_voltage(2.0);
+}
void AutonomousActor::Reset() {
InitializeEncoders();
@@ -62,25 +66,45 @@
}
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams *params) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
Reset();
+ AOS_LOG(INFO, "Params are %d\n", params->mode());
if (alliance_ == aos::Alliance::kInvalid) {
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
return false;
}
+ if (FLAGS_spline_auto) {
+ SplineAuto();
+ } else {
+ return DriveFwd();
+ }
+ return true;
+}
+void AutonomousActor::SplineAuto() {
SplineHandle spline1 = PlanSpline(std::bind(AutonomousSplines::BasicSSpline,
std::placeholders::_1, alliance_),
SplineDirection::kForward);
- if (!spline1.WaitForPlan()) return true;
+ if (!spline1.WaitForPlan()) return;
spline1.Start();
- if (!spline1.WaitForSplineDistanceRemaining(0.02)) return true;
-
- AOS_LOG(INFO, "Params are %d\n", params->mode());
- return true;
+ if (!spline1.WaitForSplineDistanceRemaining(0.02)) return;
}
+ProfileParametersT MakeProfileParametersT(const float max_velocity,
+ const float max_acceleration) {
+ ProfileParametersT params;
+ params.max_velocity = max_velocity;
+ params.max_acceleration = max_acceleration;
+ return params;
+}
+
+bool AutonomousActor::DriveFwd() {
+ const ProfileParametersT kDrive = MakeProfileParametersT(0.1f, 0.5f);
+ const ProfileParametersT kTurn = MakeProfileParametersT(5.0f, 15.0f);
+ StartDrive(0.5, 0.0, kDrive, kTurn);
+ return WaitForDriveDone();
+}
} // namespace actors
} // namespace y2020
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 747fa8d..4ad04d2 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -20,6 +20,8 @@
private:
void Reset();
+ void SplineAuto();
+ bool DriveFwd();
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
diff --git a/y2020/y2020_laptop.json b/y2020/y2020_laptop.json
index bb835c0..9ebac58 100644
--- a/y2020/y2020_laptop.json
+++ b/y2020/y2020_laptop.json
@@ -106,7 +106,7 @@
"source_node": "laptop",
"frequency": 50,
"num_senders": 20,
- "max_size": 2048
+ "max_size": 4096
},
{
"name": "/laptop/aos",
diff --git a/y2020/y2020_pi_template.json b/y2020/y2020_pi_template.json
index b504599..db1de5e 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -7,7 +7,7 @@
"source_node": "pi{{ NUM }}",
"frequency": 50,
"num_senders": 20,
- "max_size": 2048
+ "max_size": 4096
},
{
"name": "/pi{{ NUM }}/aos",
@@ -18,6 +18,20 @@
},
{
"name": "/pi{{ NUM }}/aos",
+ "type": "aos.starter.Status",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 50,
+ "num_senders": 20
+ },
+ {
+ "name": "/pi{{ NUM }}/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/pi{{ NUM }}/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi{{ NUM }}",
"frequency": 2,
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index d463848..2e29eee 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -19,7 +19,7 @@
"source_node": "roborio",
"frequency": 50,
"num_senders": 20,
- "max_size": 2048
+ "max_size": 4096
},
{
"name": "/roborio/aos",
@@ -30,6 +30,20 @@
},
{
"name": "/roborio/aos",
+ "type": "aos.starter.Status",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "roborio",
"frequency": 2,
@@ -45,24 +59,28 @@
{
"name": "/roborio/aos/remote_timestamps/pi1",
"type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
"name": "/roborio/aos/remote_timestamps/pi2",
"type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
"name": "/roborio/aos/remote_timestamps/pi3",
"type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},
{
"name": "/roborio/aos/remote_timestamps/pi4",
"type": "aos.message_bridge.RemoteMessage",
+ "frequency": 200,
"logger": "NOT_LOGGED",
"source_node": "roborio"
},