Send drivetrain message in sensor_reader
This moves the logic from sending our drivetrain message in
sensor_reader from wpilib_interface to a helper function in
sensor_reader.h
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Id5eabc4dceca77038eb656bfe4507fed49031d90
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index bbcc3b5..70d5c35 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -84,6 +84,32 @@
return monotonic_clock::epoch() + (new_fpga_time + *fpga_offset);
}
+void SensorReader::SendDrivetrainPosition(
+ aos::Sender<control_loops::drivetrain::Position> drivetrain_position_sender,
+ std::function<double(double input)> velocity_translate,
+ std::function<double(double input)> encoder_to_meters, bool left_inverted,
+ bool right_inverted) {
+ auto builder = drivetrain_position_sender.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+
+ drivetrain_builder.add_left_encoder(
+ (left_inverted ? -1.0 : 1.0) *
+ encoder_to_meters(drivetrain_left_encoder_->GetRaw()));
+ drivetrain_builder.add_left_speed(
+ (left_inverted ? -1.0 : 1.0) *
+ velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+
+ drivetrain_builder.add_right_encoder(
+ (right_inverted ? -1.0 : 1.0) *
+ encoder_to_meters(drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed(
+ (right_inverted ? -1.0 : 1.0) *
+ velocity_translate(drivetrain_right_encoder_->GetPeriod()));
+
+ builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
+}
+
void SensorReader::DoStart() {
Start();
if (dma_synchronizer_) {