Allow for disabling of shifting using hall effects.
There is a new constant that, if set to true, will make it simply
set the shifter pistons and not try to velocity match. This is
for the third robot, in which the analog hall effect sensors on
the shifters may never end up working correctly.
Change-Id: I344e4cf9fcca4fd32347da56d8b48e8a9fd5332e
diff --git a/bot3/control_loops/drivetrain/drivetrain.cc b/bot3/control_loops/drivetrain/drivetrain.cc
index bf81510..3a40836 100644
--- a/bot3/control_loops/drivetrain/drivetrain.cc
+++ b/bot3/control_loops/drivetrain/drivetrain.cc
@@ -292,38 +292,20 @@
static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
- double shifter_position, double velocity) {
+ double shifter_position, double velocity, Gear gear) {
// TODO(austin): G_high, G_low and kWheelRadius
const double avg_hall_effect =
(hall_effect.clear_high + hall_effect.clear_low) / 2.0;
- if (shifter_position > avg_hall_effect) {
+ const bool use_high =
+ kBot3SimpleShifting ? gear == HIGH : shifter_position > avg_hall_effect;
+ if (use_high) {
return velocity / kBot3HighGearRatio / kWheelRadius;
} else {
return velocity / kBot3LowGearRatio / kWheelRadius;
}
}
- 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 =
- MotorSpeed(hall_effect, 1.0, ::std::abs(velocity));
-
- double high_torque = ((12.0 - high_omega / Kv) * Kt / kR);
- double low_torque = ((12.0 - low_omega / Kv) * Kt / kR);
- double high_power = high_torque * high_omega;
- double low_power = low_torque * low_omega;
-
- // TODO(aschuh): Do this right!
- if ((current == HIGH || high_power > low_power + 160) &&
- ::std::abs(velocity) > 0.14) {
- return HIGH;
- } else {
- return LOW;
- }
- }
-
void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
const double kWheelNonLinearity = 0.3;
// Apply a sin function that's scaled to make it feel better.
@@ -342,26 +324,11 @@
// TODO(austin): Fix the upshift logic to include states.
Gear requested_gear;
- if (false) {
- const double current_left_velocity =
- (position_.left_encoder - last_position_.left_encoder) /
- position_time_delta_;
- const double current_right_velocity =
- (position_.right_encoder - last_position_.right_encoder) /
- position_time_delta_;
+ requested_gear = highgear ? HIGH : LOW;
- Gear left_requested =
- ComputeGear(kBot3LeftDriveShifter, current_left_velocity, left_gear_);
- Gear right_requested =
- ComputeGear(kBot3RightDriveShifter, current_right_velocity, right_gear_);
- requested_gear =
- (left_requested == HIGH || right_requested == HIGH) ? HIGH : LOW;
- } else {
- requested_gear = highgear ? HIGH : LOW;
- }
-
- const Gear shift_up = SHIFTING_UP;
- const Gear shift_down = SHIFTING_DOWN;
+ // Can be set to HIGH and LOW instead if we want to use simple shifting.
+ const Gear shift_up = kBot3SimpleShifting ? HIGH : SHIFTING_UP;
+ const Gear shift_down = kBot3SimpleShifting ? LOW : SHIFTING_DOWN;
if (left_gear_ != requested_gear) {
if (IsInGear(left_gear_)) {
@@ -394,6 +361,7 @@
}
}
}
+
void SetPosition(const Drivetrain::Position *position) {
if (position == NULL) {
++stale_count_;
@@ -414,7 +382,8 @@
(kBot3RightDriveShifter.clear_high +
kBot3RightDriveShifter.clear_low) / 2.0;
- if (position->left_shifter_position < left_middle_shifter_position) {
+ if (position->left_shifter_position < left_middle_shifter_position ||
+ left_gear_ == LOW) {
if (position->right_shifter_position < right_middle_shifter_position ||
right_gear_ == LOW) {
gear_logging.left_loop_high = false;
@@ -427,7 +396,7 @@
}
} else {
if (position->right_shifter_position < right_middle_shifter_position ||
- left_gear_ == LOW) {
+ right_gear_ == LOW) {
gear_logging.left_loop_high = true;
gear_logging.right_loop_high = false;
loop_->set_controller_index(gear_logging.controller_index = 2);
@@ -525,11 +494,13 @@
const double left_motor_speed =
MotorSpeed(kBot3LeftDriveShifter,
position_.left_shifter_position,
- current_left_velocity);
+ current_left_velocity,
+ left_gear_);
const double right_motor_speed =
MotorSpeed(kBot3RightDriveShifter,
position_.right_shifter_position,
- current_right_velocity);
+ current_right_velocity,
+ right_gear_);
{
CIMLogging logging;
diff --git a/bot3/control_loops/drivetrain/drivetrain_constants.h b/bot3/control_loops/drivetrain/drivetrain_constants.h
index 8563cbef..28848b0 100644
--- a/bot3/control_loops/drivetrain/drivetrain_constants.h
+++ b/bot3/control_loops/drivetrain/drivetrain_constants.h
@@ -17,6 +17,9 @@
constexpr double kBot3LowGearRatio = 14.0 / 60.0 * 17.0 / 50.0;
constexpr double kBot3HighGearRatio = 30.0 / 44.0 * 17.0 / 50.0;
+// If this is true, we don't use the analog hall effects for shifting.
+constexpr bool kBot3SimpleShifting = true;
+
} // control_loops
} // bot3
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index dba72d4..8cc1e62 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -409,7 +409,8 @@
const double right_middle_shifter_position =
(values.right_drive.clear_high + values.right_drive.clear_low) / 2.0;
- if (position->left_shifter_position < left_middle_shifter_position) {
+ if (position->left_shifter_position < left_middle_shifter_position ||
+ left_gear_ == LOW) {
if (position->right_shifter_position < right_middle_shifter_position ||
right_gear_ == LOW) {
gear_logging.left_loop_high = false;
@@ -422,7 +423,7 @@
}
} else {
if (position->right_shifter_position < right_middle_shifter_position ||
- left_gear_ == LOW) {
+ right_gear_ == LOW) {
gear_logging.left_loop_high = true;
gear_logging.right_loop_high = false;
loop_->set_controller_index(gear_logging.controller_index = 2);