Modify code for single shifter hall effect.
The third robot has only one hall effect sensor sensing the
entire range of the shifter piston.
Change-Id: I6008c7b920d21da024c90ee1eb40dc2c7e400b62
diff --git a/bot3/control_loops/drivetrain/drivetrain.cc b/bot3/control_loops/drivetrain/drivetrain.cc
index ae313e5..bf81510 100644
--- a/bot3/control_loops/drivetrain/drivetrain.cc
+++ b/bot3/control_loops/drivetrain/drivetrain.cc
@@ -20,6 +20,7 @@
#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "bot3/shifter_hall_effect.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/queues/other_sensors.q.h"
@@ -290,7 +291,7 @@
}
static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
- static double MotorSpeed(const ::frc971::constants::ShifterHallEffect &hall_effect,
+ static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
double shifter_position, double velocity) {
// TODO(austin): G_high, G_low and kWheelRadius
const double avg_hall_effect =
@@ -303,7 +304,7 @@
}
}
- Gear ComputeGear(const ::frc971::constants::ShifterHallEffect &hall_effect,
+ Gear ComputeGear(const constants::ShifterHallEffect &hall_effect,
double velocity, Gear current) {
const double low_omega = MotorSpeed(hall_effect, 0.0, ::std::abs(velocity));
const double high_omega =
diff --git a/bot3/control_loops/drivetrain/drivetrain_constants.h b/bot3/control_loops/drivetrain/drivetrain_constants.h
index 9d49d59..95a74f5 100644
--- a/bot3/control_loops/drivetrain/drivetrain_constants.h
+++ b/bot3/control_loops/drivetrain/drivetrain_constants.h
@@ -1,17 +1,17 @@
#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_CONSTANTS_H_
#define BOT3_CONTROL_LOOPS_DRIVETRAIN_CONSTANTS_H_
+#include "bot3/shifter_hall_effect.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/shifter_hall_effect.h"
namespace bot3 {
namespace control_loops {
// TODO(danielp): Figure out the real values for these constants.
-constexpr ::frc971::constants::ShifterHallEffect kBot3LeftDriveShifter =
- {555, 657, 660, 560, 0.2, 0.7};
-constexpr ::frc971::constants::ShifterHallEffect kBot3RightDriveShifter =
- {555, 660, 644, 552, 0.2, 0.7};
+constexpr constants::ShifterHallEffect kBot3LeftDriveShifter =
+ {170, 475, 1.2, 1.0};
+constexpr constants::ShifterHallEffect kBot3RightDriveShifter =
+ {177, 486, 1.2, 1.0};
constexpr double kBot3TurnWidth = 0.5;
constexpr double kBot3DrivetrainDoneDistance = 0.02;
diff --git a/bot3/input/sensor_receiver.cc b/bot3/input/sensor_receiver.cc
index 6aec89d..c42e609 100644
--- a/bot3/input/sensor_receiver.cc
+++ b/bot3/input/sensor_receiver.cc
@@ -12,8 +12,8 @@
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
#include "bot3/queues/to_log.q.h"
+#include "bot3/shifter_hall_effect.h"
#include "frc971/queues/other_sensors.q.h"
-#include "frc971/shifter_hall_effect.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -64,21 +64,10 @@
return in / 16.0 / 1000.0 / (180.0 / M_PI);
}
-double hall_translate(const ::frc971::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 hall_translate(const constants::ShifterHallEffect & k,
+ uint16_t in_value) {
+ return (in_value - static_cast<double>(k.low)) /
+ static_cast<double>(k.high - k.low);
}
void PacketReceived(const ::bbb::DataStruct *data,
@@ -137,11 +126,9 @@
.right_encoder(drivetrain_translate(data->main.right_drive))
.left_encoder(-drivetrain_translate(data->main.left_drive))
.left_shifter_position(hall_translate(control_loops::kBot3LeftDriveShifter,
- data->main.low_left_drive_hall,
- data->main.high_left_drive_hall))
+ data->main.low_left_drive_hall))
.right_shifter_position(hall_translate(control_loops::kBot3RightDriveShifter,
- data->main.low_right_drive_hall,
- data->main.high_right_drive_hall))
+ data->main.low_right_drive_hall))
.battery_voltage(battery_translate(data->main.battery_voltage_high,
data->main.battery_voltage_low))
.Send();
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index e85351e..dba72d4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -12,12 +12,12 @@
#include "aos/common/logging/queue_logging.h"
#include "aos/common/logging/matrix_logging.h"
+#include "frc971/constants.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/other_sensors.q.h"
-#include "frc971/constants.h"
#include "frc971/shifter_hall_effect.h"
using frc971::sensors::gyro_reading;