Update Hood to use AbsoluteAndAbsolute position
Change-Id: I3e88718560a6e3983681a3430dc31b5431c1a743
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 25e7592..39b83fa 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -102,6 +102,29 @@
encoder_ratio * (2.0 * M_PI);
}
+ // Copies an AbsoluteEncoderAndPotentiometer to an AbsoluteAndAbsolutePosition
+ // with the correct unit and direction changes.
+ void CopyPosition(const ::frc971::wpilib::AbsoluteAndAbsoluteEncoder &encoder,
+ ::frc971::AbsoluteAndAbsolutePositionT *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ double single_turn_encoder_ratio, bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+
+ position->absolute_encoder =
+ (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
+ : encoder.ReadAbsoluteEncoder()) *
+ encoder_ratio * (2.0 * M_PI);
+
+ position->single_turn_absolute_encoder =
+ (reverse ? (1.0 - encoder.ReadSingleTurnAbsoluteEncoder())
+ : encoder.ReadSingleTurnAbsoluteEncoder()) *
+ single_turn_encoder_ratio * (2.0 * M_PI);
+ }
+
// Copies a DMAEdgeCounter to a HallEffectAndPosition with the correct unit
// and direction changes.
void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
@@ -128,11 +151,10 @@
// Copies a Absolute Encoder with the correct unit
// and direction changes.
- void CopyPosition(
- const ::frc971::wpilib::AbsoluteEncoder &encoder,
- ::frc971::AbsolutePositionT *position,
- double encoder_counts_per_revolution, double encoder_ratio,
- bool reverse) {
+ void CopyPosition(const ::frc971::wpilib::AbsoluteEncoder &encoder,
+ ::frc971::AbsolutePositionT *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.ReadRelativeEncoder(),
@@ -145,12 +167,11 @@
encoder_ratio * (2.0 * M_PI);
}
- void CopyPosition(
- const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
- ::frc971::PotAndIndexPositionT *position,
- ::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> potentiometer_translate, bool reverse,
- double pot_offset) {
+ void CopyPosition(const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
+ ::frc971::PotAndIndexPositionT *position,
+ ::std::function<double(int32_t)> encoder_translate,
+ ::std::function<double(double)> potentiometer_translate,
+ bool reverse, double pot_offset) {
const double multiplier = reverse ? -1.0 : 1.0;
position->encoder =
multiplier * encoder_translate(encoder.polled_encoder_value());