Create CopyPosition function for RelativePosition.
Change-Id: If91e63ceed3eba7cee1f08e29d54e971a58fddfe
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 3e24fd7..25e7592 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -166,6 +166,18 @@
position->index_pulses = encoder.index_posedge_count();
}
+ // Copies a relative encoder.
+ void CopyPosition(const ::frc::Encoder &encoder,
+ ::frc971::RelativePositionT *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.GetRaw(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ }
+
double encoder_translate(int32_t value, double counts_per_revolution,
double ratio) {
return static_cast<double>(value) / counts_per_revolution * ratio *