Create an enum for sender errors
Will replace usages of bools, and will now currently only be used
for indicating that messages were sent too fast
After we merge this commit we will replace this enum with a general
Status class for all of aos, similar to absl::Status.
Change-Id: I4b5b2e7685744b3c6826a241cd3c84190eaa96ee
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 928d418..62b77c4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -526,7 +526,10 @@
builder.add_down_estimator(down_estimator_state_offset);
builder.add_localizer(localizer_offset);
builder.add_zeroing(zeroer_offset);
- status->Send(builder.Finish());
+
+ builder.add_send_failures(status_failure_counter_.failures());
+
+ status_failure_counter_.Count(status->Send(builder.Finish()));
}
// If the filters aren't ready/valid, then disable all outputs (currently,
@@ -568,7 +571,7 @@
}
if (output) {
- output->Send(Output::Pack(*output->fbb(), &output_struct));
+ output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index efaf53d..52ae605 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -2,9 +2,8 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_H_
#include "Eigen/Dense"
-#include "frc971/control_loops/control_loop.h"
-#include "frc971/control_loops/polytope.h"
#include "aos/util/log_interval.h"
+#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -20,6 +19,7 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/polytope.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/wpilib/imu_batch_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
@@ -185,6 +185,8 @@
LineFollowDrivetrain dt_line_follow_;
bool has_been_enabled_ = false;
+
+ aos::SendFailureCounter status_failure_counter_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d658e21..3ad4776 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -183,7 +183,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(2));
VerifyNearGoal();
@@ -199,7 +200,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Sanity check that the drivetrain is indeed commanding voltage while the IMU
@@ -235,7 +237,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
drivetrain_plant_.set_left_voltage_offset(1.0);
drivetrain_plant_.set_right_voltage_offset(1.0);
@@ -251,7 +254,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(-1.0);
goal_builder.add_right_goal(1.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
if (i > 20 && i < 200) {
@@ -281,7 +285,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(4.0);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
@@ -305,7 +310,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(3.9);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 500; ++i) {
RunFor(dt());
@@ -366,7 +372,8 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -408,7 +415,8 @@
goal_builder.add_right_goal(1.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -450,7 +458,8 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto start_time = monotonic_now();
@@ -473,7 +482,8 @@
goal_builder.add_throttle(1.0);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(1));
@@ -486,7 +496,8 @@
goal_builder.add_throttle(-0.3);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(1));
@@ -499,7 +510,8 @@
goal_builder.add_throttle(0.0);
goal_builder.add_highgear(true);
goal_builder.add_quickturn(false);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(10));
@@ -526,7 +538,8 @@
goal_builder.add_right_goal(4.0);
goal_builder.add_linear(linear_offset);
goal_builder.add_angular(angular_offset);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
const auto end_time = monotonic_now() + chrono::seconds(4);
@@ -566,7 +579,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -576,7 +590,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -611,7 +626,8 @@
spline_goal_builder.add_drive_spline_backwards(true);
spline_goal_builder.add_spline(multispline_offset);
- EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -620,7 +636,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Check that we are right on the spline at the start (otherwise the feedback
@@ -669,7 +686,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- EXPECT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
@@ -677,7 +695,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- EXPECT_TRUE(builder.Send(goal_builder.Finish()));
+ EXPECT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -709,14 +728,16 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -728,7 +749,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 100; ++i) {
RunFor(dt());
@@ -769,7 +791,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
@@ -777,7 +800,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -789,7 +813,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -799,7 +824,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -828,7 +854,8 @@
localizer_control_builder.add_x(drivetrain_plant_.state()(0));
localizer_control_builder.add_y(drivetrain_plant_.state()(1));
localizer_control_builder.add_theta(drivetrain_plant_.state()(2));
- ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
+ ASSERT_EQ(builder.Send(localizer_control_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = trajectory_goal_sender_.MakeBuilder();
@@ -852,7 +879,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(GetParam());
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -861,7 +889,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -898,7 +927,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -907,7 +937,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -947,7 +978,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -956,7 +988,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(5000));
@@ -1006,7 +1039,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1015,7 +1049,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(4000));
@@ -1047,7 +1082,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1056,7 +1092,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1086,7 +1123,8 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1096,7 +1134,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1128,14 +1167,16 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1153,7 +1194,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(500));
@@ -1180,14 +1222,16 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1222,7 +1266,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(1000));
@@ -1250,7 +1295,8 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Now execute it.
@@ -1259,7 +1305,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1292,7 +1339,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Second spline goal
@@ -1318,14 +1366,16 @@
spline_goal_builder.add_spline_idx(2);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1335,7 +1385,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(2);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(4000));
@@ -1367,7 +1418,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (int i = 0; i < 100; ++i) {
@@ -1402,7 +1454,8 @@
spline_goal_builder.add_spline_idx(1);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::milliseconds(2000));
@@ -1412,7 +1465,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(1);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
WaitForTrajectoryExecution();
@@ -1434,7 +1488,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::SPLINE_FOLLOWER);
goal_builder.add_spline_handle(kRunSpline);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
for (size_t spline_index = 0;
spline_index < DrivetrainLoop::kNumSplineFetchers + kExtraSplines;
@@ -1460,7 +1515,8 @@
spline_goal_builder.add_spline_idx(spline_index);
spline_goal_builder.add_drive_spline_backwards(false);
spline_goal_builder.add_spline(multispline_offset);
- ASSERT_TRUE(builder.Send(spline_goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(spline_goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
// Run for at least 2 iterations. Because of how the logic works, there will
// actually typically be a single iteration where we store kNumStoredSplines
// + 1.
@@ -1504,7 +1560,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -1541,7 +1598,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_controller_type(ControllerType::LINE_FOLLOWER);
goal_builder.add_throttle(0.5);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(5));
@@ -1564,7 +1622,8 @@
localizer_control_builder.add_x(9.0);
localizer_control_builder.add_y(7.0);
localizer_control_builder.add_theta(1.0);
- ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
+ ASSERT_EQ(builder.Send(localizer_control_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(dt());
@@ -1586,7 +1645,8 @@
goal_builder.add_controller_type(ControllerType::MOTION_PROFILE);
goal_builder.add_left_goal(4.0);
goal_builder.add_right_goal(4.0);
- ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ ASSERT_EQ(builder.Send(goal_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
RunFor(chrono::seconds(2));
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index fd3ff0e..f886bb2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -259,6 +259,9 @@
localizer:LocalizerState (id: 26);
zeroing:ImuZeroerState (id: 27);
+
+ // Total number of status send failures.
+ send_failures:uint64 (id: 28);
}
root_type Status;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index aabd1c6..fef0d13 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -7,6 +7,7 @@
#include "frc971/control_loops/drivetrain/trajectory.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "gflags/gflags.h"
+#include "glog/logging.h"
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
@@ -168,7 +169,8 @@
status_builder.add_x(state_.x());
status_builder.add_y(state_.y());
status_builder.add_theta(state_(2));
- builder.Send(status_builder.Finish());
+ CHECK_EQ(builder.Send(status_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
void DrivetrainSimulation::SendPositionMessage() {
@@ -184,7 +186,8 @@
position_builder.add_right_encoder(right_encoder);
position_builder.add_left_shifter_position(left_gear_high_ ? 1.0 : 0.0);
position_builder.add_right_shifter_position(right_gear_high_ ? 1.0 : 0.0);
- builder.Send(position_builder.Finish());
+ CHECK_EQ(builder.Send(position_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
}
@@ -263,7 +266,8 @@
frc971::IMUValuesBatch::Builder imu_values_batch_builder =
builder.MakeBuilder<frc971::IMUValuesBatch>();
imu_values_batch_builder.add_readings(imu_values_offset);
- builder.Send(imu_values_batch_builder.Finish());
+ CHECK_EQ(builder.Send(imu_values_batch_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
// Simulates the drivetrain moving for one timestep.
diff --git a/frc971/control_loops/drivetrain/trajectory_generator.cc b/frc971/control_loops/drivetrain/trajectory_generator.cc
index 16b6809..d022b34 100644
--- a/frc971/control_loops/drivetrain/trajectory_generator.cc
+++ b/frc971/control_loops/drivetrain/trajectory_generator.cc
@@ -22,7 +22,8 @@
aos::Sender<fb::Trajectory>::Builder builder =
trajectory_sender_.MakeBuilder();
- CHECK(builder.Send(trajectory.Serialize(builder.fbb())));
+ CHECK_EQ(builder.Send(trajectory.Serialize(builder.fbb())),
+ aos::RawSender::Error::kOk);
}
} // namespace drivetrain