Fixed drivetrain hall effects to use the dual hall effect setup.
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index debef38..4aa2a10 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -86,8 +86,10 @@
int32_t shooter_position, pusher_distal_posedge_position,
pusher_proximal_posedge_position;
- uint16_t left_drive_hall;
- uint16_t right_drive_hall;
+ uint16_t low_left_drive_hall;
+ uint16_t high_left_drive_hall;
+ uint16_t low_right_drive_hall;
+ uint16_t high_right_drive_hall;
uint16_t battery_voltage;
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index 57e3d66..ad4f45b 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -118,8 +118,10 @@
void robot_fill_packet(struct DataStruct *packet) {
packet->main.left_drive = encoder_read(6);
packet->main.right_drive = encoder_read(5);
- packet->main.left_drive_hall = analog_get(7);
- packet->main.right_drive_hall = analog_get(0);
+ packet->main.low_left_drive_hall = analog_get(7);
+ packet->main.low_right_drive_hall = analog_get(0);
+ packet->main.high_left_drive_hall = analog_get(6);
+ packet->main.high_right_drive_hall = analog_get(1);
packet->main.ultrasonic_pulse_length = ultrasonic_pulse_length;
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 48519ff..06e1556 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -31,11 +31,11 @@
const double kPracticeLowGearRatio = 16.0 / 60.0 * 19.0 / 50.0;
const double kPracticeHighGearRatio = 28.0 / 48.0 * 19.0 / 50.0;
-const ShifterHallEffect kCompLeftDriveShifter{0.83, 2.32, 1.2, 1.0};
-const ShifterHallEffect kCompRightDriveShifter{0.865, 2.375, 1.2, 1.0};
+const ShifterHallEffect kCompRightDriveShifter{525, 635, 603, 529, 0.3, 0.7};
+const ShifterHallEffect kCompLeftDriveShifter{525, 645, 620, 533, 0.3, 0.7};
-const ShifterHallEffect kPracticeRightDriveShifter{2.575, 3.16, 0.98, 0.40};
-const ShifterHallEffect kPracticeLeftDriveShifter{2.57, 3.15, 0.98, 0.4};
+const ShifterHallEffect kPracticeRightDriveShifter{550, 640, 635, 550, 0.2, 0.7};
+const ShifterHallEffect kPracticeLeftDriveShifter{540, 620, 640, 550, 0.2, 0.7};
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
@@ -71,7 +71,7 @@
0.9, // start_fine_tune_pos
4.0,
},
- {0.01, 0.1}, // shooter_action
+ {0.07, 0.15}, // shooter_action
0.02, // drivetrain done delta
5.0 // drivetrain max speed
};
@@ -107,7 +107,7 @@
4.0,
},
//TODO(james): Get realer numbers for shooter_action.
- {0.01, 0.1}, // shooter_action
+ {0.07, 0.15}, // shooter_action
0.02, // drivetrain done delta
5.0 // drivetrain max speed
};
@@ -144,7 +144,7 @@
4.000000,
},
//TODO(james): Get realer numbers for shooter_action.
- {0.01, 0.1}, // shooter_action
+ {0.07, 0.15}, // shooter_action
0.02, // drivetrain done delta
5.0 // drivetrain max speed
};
diff --git a/frc971/constants.h b/frc971/constants.h
index 775ee10..1bf2dc5 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -17,7 +17,9 @@
// Contains the voltages for an analog hall effect sensor on a shifter.
struct ShifterHallEffect {
// The numbers to use for scaling raw voltages to 0-1.
- double high, low;
+ // Low is near 0.0, high is near 1.0
+ double low_gear_middle, low_gear_low;
+ double high_gear_high, high_gear_middle;
// The numbers for when the dog is clear of each gear.
double clear_high, clear_low;
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();