Make talonfx use static flatbuffer api
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I3ebc2e27c4bf3f5f715ce02fd6d80de63240c8e5
diff --git a/frc971/wpilib/talonfx.cc b/frc971/wpilib/talonfx.cc
index dd30f9d..5e77618 100644
--- a/frc971/wpilib/talonfx.cc
+++ b/frc971/wpilib/talonfx.cc
@@ -113,26 +113,13 @@
return status;
}
-
-void TalonFX::SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+void TalonFX::SerializePosition(control_loops::CANTalonFXStatic *can_talonfx,
double gear_ratio) {
- control_loops::CANTalonFX::Builder builder(*fbb);
- builder.add_id(device_id_);
- builder.add_device_temp(device_temp());
- builder.add_supply_voltage(supply_voltage());
- builder.add_supply_current(supply_current());
- builder.add_torque_current(torque_current());
- builder.add_duty_cycle(duty_cycle());
- builder.add_position(position() * gear_ratio);
-
- last_position_offset_ = builder.Finish();
-}
-
-std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
-TalonFX::TakeOffset() {
- auto option_offset = last_position_offset_;
-
- last_position_offset_.reset();
-
- return option_offset;
+ can_talonfx->set_id(device_id_);
+ can_talonfx->set_device_temp(device_temp());
+ can_talonfx->set_supply_voltage(supply_voltage());
+ can_talonfx->set_supply_current(supply_current());
+ can_talonfx->set_torque_current(torque_current());
+ can_talonfx->set_duty_cycle(duty_cycle());
+ can_talonfx->set_position(position() * gear_ratio);
}
diff --git a/frc971/wpilib/talonfx.h b/frc971/wpilib/talonfx.h
index 603f4a2..c25fa81 100644
--- a/frc971/wpilib/talonfx.h
+++ b/frc971/wpilib/talonfx.h
@@ -11,7 +11,7 @@
#include "aos/commonmath.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/can_talonfx_static.h"
namespace control_loops = ::frc971::control_loops;
@@ -48,11 +48,9 @@
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
// The position of the TalonFX output shaft is multiplied by gear_ratio
- void SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+ void SerializePosition(control_loops::CANTalonFXStatic *can_falcon,
double gear_ratio);
- std::optional<flatbuffers::Offset<control_loops::CANTalonFX>> TakeOffset();
-
int device_id() const { return device_id_; }
float device_temp() const { return device_temp_.GetValue().value(); }
float supply_voltage() const { return supply_voltage_.GetValue().value(); }
@@ -101,9 +99,6 @@
double stator_current_limit_;
double supply_current_limit_;
-
- std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
- last_position_offset_;
};
} // namespace wpilib
} // namespace frc971
diff --git a/y2023_bot4/wpilib_interface.cc b/y2023_bot4/wpilib_interface.cc
index 8bac186..9dd3604 100644
--- a/y2023_bot4/wpilib_interface.cc
+++ b/y2023_bot4/wpilib_interface.cc
@@ -10,7 +10,7 @@
#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2023_bot4/constants.h"
-#include "y2023_bot4/drivetrain_can_position_generated.h"
+#include "y2023_bot4/drivetrain_can_position_static.h"
#include "y2023_bot4/drivetrain_position_generated.h"
DEFINE_bool(ctre_diag_server, false,
@@ -43,21 +43,13 @@
return builder.Finish();
}
-flatbuffers::Offset<SwerveModuleCANPosition> can_module_offset(
- SwerveModuleCANPosition::Builder builder,
- std::shared_ptr<SwerveModule> module) {
- std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
- rotation_offset = module->rotation->TakeOffset();
- std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
- translation_offset = module->translation->TakeOffset();
+void populate_can_module(SwerveModuleCANPositionStatic *can_position,
+ std::shared_ptr<SwerveModule> module) {
+ auto rotation = can_position->add_rotation();
+ module->rotation->SerializePosition(rotation, 1.0);
- CHECK(rotation_offset.has_value());
- CHECK(translation_offset.has_value());
-
- builder.add_rotation(rotation_offset.value());
- builder.add_translation(translation_offset.value());
-
- return builder.Finish();
+ auto translation = can_position->add_translation();
+ module->translation->SerializePosition(translation, 1.0);
}
constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
@@ -225,8 +217,8 @@
falcons.push_back(back_right->rotation);
falcons.push_back(back_right->translation);
- aos::Sender<AbsoluteCANPosition> can_position_sender =
- can_sensor_reader_event_loop.MakeSender<AbsoluteCANPosition>(
+ aos::Sender<AbsoluteCANPositionStatic> can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<AbsoluteCANPositionStatic>(
"/drivetrain");
CANSensorReader can_sensor_reader(
@@ -236,31 +228,24 @@
// TODO(max): use status properly in the flatbuffer.
(void)status;
- auto builder = can_position_sender.MakeBuilder();
+ aos::Sender<AbsoluteCANPositionStatic>::StaticBuilder builder =
+ can_position_sender.MakeStaticBuilder();
for (auto falcon : falcons) {
falcon->RefreshNontimesyncedSignals();
- falcon->SerializePosition(builder.fbb(), 1.0);
}
- auto front_left_offset = can_module_offset(
- builder.MakeBuilder<SwerveModuleCANPosition>(), front_left);
- auto front_right_offset = can_module_offset(
- builder.MakeBuilder<SwerveModuleCANPosition>(), front_right);
- auto back_left_offset = can_module_offset(
- builder.MakeBuilder<SwerveModuleCANPosition>(), back_left);
- auto back_right_offset = can_module_offset(
- builder.MakeBuilder<SwerveModuleCANPosition>(), back_right);
+ auto front_left_flatbuffer = builder->add_front_left();
+ auto front_right_flatbuffer = builder->add_front_right();
+ auto back_left_flatbuffer = builder->add_back_left();
+ auto back_right_flatbuffer = builder->add_back_right();
- AbsoluteCANPosition::Builder can_position_builder =
- builder.MakeBuilder<AbsoluteCANPosition>();
+ populate_can_module(front_left_flatbuffer, front_left);
+ populate_can_module(front_right_flatbuffer, front_right);
+ populate_can_module(back_left_flatbuffer, back_left);
+ populate_can_module(back_right_flatbuffer, back_right);
- can_position_builder.add_front_left(front_left_offset);
- can_position_builder.add_front_right(front_right_offset);
- can_position_builder.add_back_left(back_left_offset);
- can_position_builder.add_back_right(back_right_offset);
-
- builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ builder.CheckOk(builder.Send());
});
AddLoop(&can_sensor_reader_event_loop);
diff --git a/y2024_defense/wpilib_interface.cc b/y2024_defense/wpilib_interface.cc
index f3a0b8f..d599813 100644
--- a/y2024_defense/wpilib_interface.cc
+++ b/y2024_defense/wpilib_interface.cc
@@ -40,6 +40,7 @@
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/can_configuration_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_static.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/input/robot_state_generated.h"
#include "frc971/queues/gyro_generated.h"
@@ -68,7 +69,7 @@
using ::frc971::CANConfiguration;
using ::y2024_defense::constants::Values;
-using frc971::control_loops::drivetrain::CANPosition;
+using frc971::control_loops::drivetrain::CANPositionStatic;
using frc971::wpilib::TalonFX;
using std::make_unique;
@@ -312,44 +313,28 @@
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
- aos::Sender<CANPosition> can_position_sender =
- can_sensor_reader_event_loop.MakeSender<CANPosition>("/drivetrain");
+ aos::Sender<CANPositionStatic> can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<CANPositionStatic>(
+ "/drivetrain");
frc971::wpilib::CANSensorReader can_sensor_reader(
&can_sensor_reader_event_loop, std::move(signals_registry), falcons,
[falcons, &can_position_sender](ctre::phoenix::StatusCode status) {
- auto builder = can_position_sender.MakeBuilder();
- aos::SizedArray<
- flatbuffers::Offset<frc971::control_loops::CANTalonFX>, 6>
- flatbuffer_falcons;
+ aos::Sender<CANPositionStatic>::StaticBuilder builder =
+ can_position_sender.MakeStaticBuilder();
+
+ auto falcon_vector = builder->add_talonfxs();
for (auto falcon : falcons) {
falcon->SerializePosition(
- builder.fbb(), control_loops::drivetrain::kHighOutputRatio);
- std::optional<
- flatbuffers::Offset<frc971::control_loops::CANTalonFX>>
- falcon_offset = falcon->TakeOffset();
-
- CHECK(falcon_offset.has_value());
-
- flatbuffer_falcons.push_back(falcon_offset.value());
+ falcon_vector->emplace_back(),
+ control_loops::drivetrain::kHighOutputRatio);
}
- auto falcons_list =
- builder.fbb()
- ->CreateVector<
- flatbuffers::Offset<frc971::control_loops::CANTalonFX>>(
- flatbuffer_falcons);
+ builder->set_timestamp(falcons.front()->GetTimestamp());
+ builder->set_status(static_cast<int>(status));
- frc971::control_loops::drivetrain::CANPosition::Builder
- can_position_builder = builder.MakeBuilder<
- frc971::control_loops::drivetrain::CANPosition>();
-
- can_position_builder.add_talonfxs(falcons_list);
- can_position_builder.add_timestamp(falcons.front()->GetTimestamp());
- can_position_builder.add_status(static_cast<int>(status));
-
- builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ builder.CheckOk(builder.Send());
});
AddLoop(&can_sensor_reader_event_loop);