Fixed drivetrain hall effects to use the dual hall effect setup.
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 47927c4..8b01ade 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -64,9 +64,21 @@
return adc_translate(in) * kDividerBig / kDividerSmall;
}
-double hall_translate(const constants::ShifterHallEffect &k, uint16_t in) {
- const double voltage = adc_translate(in);
- return (voltage - k.low) / (k.high - k.low);
+double hall_translate(const constants::ShifterHallEffect &k, uint16_t in_low,
+ uint16_t in_high) {
+ const double low_ratio =
+ 0.5 * (in_low - static_cast<double>(k.low_gear_low)) /
+ static_cast<double>(k.low_gear_middle - k.low_gear_low);
+ const double high_ratio =
+ 0.5 + 0.5 * (in_high - static_cast<double>(k.high_gear_middle)) /
+ static_cast<double>(k.high_gear_high - k.high_gear_middle);
+
+ // Return low when we are below 1/2, and high when we are above 1/2.
+ if (low_ratio + high_ratio < 1.0) {
+ return low_ratio;
+ } else {
+ return high_ratio;
+ }
}
double claw_translate(int32_t in) {
@@ -147,9 +159,11 @@
.right_encoder(drivetrain_translate(data->main.right_drive))
.left_encoder(-drivetrain_translate(data->main.left_drive))
.left_shifter_position(hall_translate(constants::GetValues().left_drive,
- data->main.left_drive_hall))
- .right_shifter_position(hall_translate(
- constants::GetValues().right_drive, data->main.right_drive_hall))
+ data->main.low_left_drive_hall,
+ data->main.high_left_drive_hall))
+ .right_shifter_position(hall_translate(constants::GetValues().right_drive,
+ data->main.low_right_drive_hall,
+ data->main.high_right_drive_hall))
.battery_voltage(battery_translate(data->main.battery_voltage))
.Send();