Add button to set line following mode
Button is the top button on the pistol grip.
Change-Id: Id8ac2faee072a4ff35d35c394a1a4bd4090e0698
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 67ee9f3..02957a8 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -23,8 +23,6 @@
void DrivetrainInputReader::HandleDrivetrain(
const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
-
const auto wheel_and_throttle = GetWheelAndThrottle(data);
const double wheel = wheel_and_throttle.wheel;
const double wheel_velocity = wheel_and_throttle.wheel_velocity;
@@ -39,7 +37,32 @@
robot_velocity_ = drivetrain_queue.status->robot_speed;
}
- if (data.PosEdge(turn1_) || data.PosEdge(turn2_)) {
+ bool is_control_loop_driving = false;
+ bool is_line_following = false;
+
+ if (data.IsPressed(turn1_)) {
+ switch (turn1_use_) {
+ case TurnButtonUse::kControlLoopDriving:
+ is_control_loop_driving = true;
+ break;
+ case TurnButtonUse::kLineFollow:
+ is_line_following = true;
+ break;
+ }
+ }
+
+ if (data.IsPressed(turn2_)) {
+ switch (turn2_use_) {
+ case TurnButtonUse::kControlLoopDriving:
+ is_control_loop_driving = true;
+ break;
+ case TurnButtonUse::kLineFollow:
+ is_line_following = true;
+ break;
+ }
+ }
+
+ if (is_control_loop_driving) {
if (drivetrain_queue.status.get()) {
left_goal_ = drivetrain_queue.status->estimated_left_position;
right_goal_ = drivetrain_queue.status->estimated_right_position;
@@ -49,9 +72,6 @@
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
- if (data.IsPressed(turn1_) || data.IsPressed(turn2_)) {
- is_control_loop_driving = true;
- }
auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
new_drivetrain_goal->wheel = wheel;
new_drivetrain_goal->wheel_velocity = wheel_velocity;
@@ -61,7 +81,8 @@
new_drivetrain_goal->throttle_torque = throttle_torque;
new_drivetrain_goal->highgear = high_gear;
new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
- new_drivetrain_goal->controller_type = is_control_loop_driving ? 1 : 0;
+ new_drivetrain_goal->controller_type =
+ is_line_following ? 3 : (is_control_loop_driving ? 1 : 0);
new_drivetrain_goal->left_goal = current_left_goal;
new_drivetrain_goal->right_goal = current_right_goal;
@@ -192,15 +213,17 @@
const ButtonLocation kTurn1(1, 7);
const ButtonLocation kTurn2(1, 11);
std::unique_ptr<SteeringWheelDrivetrainInputReader> result(
- new SteeringWheelDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
- kQuickTurn, kTurn1, kTurn2));
+ new SteeringWheelDrivetrainInputReader(
+ kSteeringWheel, kDriveThrottle, kQuickTurn, kTurn1,
+ TurnButtonUse::kControlLoopDriving, kTurn2,
+ TurnButtonUse::kControlLoopDriving));
result.get()->set_default_high_gear(default_high_gear);
return result;
}
std::unique_ptr<PistolDrivetrainInputReader> PistolDrivetrainInputReader::Make(
- bool default_high_gear) {
+ bool default_high_gear, TopButtonUse top_button_use) {
// Pistol Grip controller
const JoystickAxis kTriggerHigh(1, 1), kTriggerLow(1, 4),
kTriggerVelocityHigh(1, 2), kTriggerVelocityLow(1, 5),
@@ -211,12 +234,23 @@
kWheelTorqueLow(2, 6);
const ButtonLocation kQuickTurn(1, 3);
- const ButtonLocation kShiftHigh(1, 1);
- const ButtonLocation kShiftLow(1, 2);
- // Nop
- const ButtonLocation kTurn1(1, 9);
- const ButtonLocation kTurn2(1, 10);
+ const ButtonLocation TopButton(1, 1);
+ const ButtonLocation SecondButton(1, 2);
+ // Non-existant button for nops.
+ const ButtonLocation DummyButton(1, 10);
+
+ // TODO(james): Make a copy assignment operator for ButtonLocation so we don't
+ // have to shoehorn in these ternary operators.
+ const ButtonLocation kTurn1 =
+ (top_button_use == TopButtonUse::kLineFollow) ? TopButton : DummyButton;
+ // Turn2 currently does nothing on the pistol grip, ever.
+ const ButtonLocation kTurn2 = DummyButton;
+ const ButtonLocation kShiftHigh =
+ (top_button_use == TopButtonUse::kShift) ? TopButton : DummyButton;
+ const ButtonLocation kShiftLow =
+ (top_button_use == TopButtonUse::kShift) ? SecondButton : DummyButton;
+
std::unique_ptr<PistolDrivetrainInputReader> result(
new PistolDrivetrainInputReader(
kWheelHigh, kWheelLow, kTriggerVelocityHigh, kTriggerVelocityLow,
@@ -239,7 +273,9 @@
std::unique_ptr<XboxDrivetrainInputReader> result(
new XboxDrivetrainInputReader(kSteeringWheel, kDriveThrottle, kQuickTurn,
- kTurn1, kTurn2));
+ kTurn1, TurnButtonUse::kControlLoopDriving,
+ kTurn2,
+ TurnButtonUse::kControlLoopDriving));
return result;
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
@@ -256,8 +292,11 @@
SteeringWheelDrivetrainInputReader::Make(dt_config.default_high_gear);
break;
case InputType::kPistol:
- drivetrain_input_reader =
- PistolDrivetrainInputReader::Make(dt_config.default_high_gear);
+ drivetrain_input_reader = PistolDrivetrainInputReader::Make(
+ dt_config.default_high_gear,
+ dt_config.pistol_grip_shift_enables_line_follow
+ ? PistolDrivetrainInputReader::TopButtonUse::kLineFollow
+ : PistolDrivetrainInputReader::TopButtonUse::kShift);
break;
case InputType::kXbox:
drivetrain_input_reader = XboxDrivetrainInputReader::Make();
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 5274cfa..80046ee 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -35,17 +35,28 @@
// joystick types.
class DrivetrainInputReader {
public:
+ // What to use the turn1/2 buttons for.
+ enum class TurnButtonUse {
+ // Use the button to enable control loop driving.
+ kControlLoopDriving,
+ // Use the button to set line following mode.
+ kLineFollow,
+ };
// Inputs driver station button and joystick locations
DrivetrainInputReader(driver_station::JoystickAxis wheel,
driver_station::JoystickAxis throttle,
driver_station::ButtonLocation quick_turn,
driver_station::ButtonLocation turn1,
- driver_station::ButtonLocation turn2)
+ TurnButtonUse turn1_use,
+ driver_station::ButtonLocation turn2,
+ TurnButtonUse turn2_use)
: wheel_(wheel),
throttle_(throttle),
quick_turn_(quick_turn),
turn1_(turn1),
- turn2_(turn2) {}
+ turn1_use_(turn1_use),
+ turn2_(turn2),
+ turn2_use_(turn2_use) {}
virtual ~DrivetrainInputReader() = default;
@@ -78,8 +89,12 @@
const driver_station::JoystickAxis wheel_;
const driver_station::JoystickAxis throttle_;
const driver_station::ButtonLocation quick_turn_;
+ // Button for enabling control loop driving.
const driver_station::ButtonLocation turn1_;
+ const TurnButtonUse turn1_use_;
+ // But for enabling line following.
const driver_station::ButtonLocation turn2_;
+ const TurnButtonUse turn2_use_;
// Structure containing the (potentially adjusted) steering and throttle
// values from the joysticks.
@@ -134,9 +149,18 @@
public:
using DrivetrainInputReader::DrivetrainInputReader;
+ // What to use the top two buttons for on the pistol grip.
+ enum class TopButtonUse {
+ // Normal shifting.
+ kShift,
+ // Line following (currently just uses top button).
+ kLineFollow,
+ };
+
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the (cheap) pistol grip controller.
- static std::unique_ptr<PistolDrivetrainInputReader> Make(bool default_high_gear);
+ static std::unique_ptr<PistolDrivetrainInputReader> Make(
+ bool default_high_gear, TopButtonUse top_button_use);
private:
PistolDrivetrainInputReader(
@@ -158,7 +182,8 @@
driver_station::ButtonLocation turn1,
driver_station::ButtonLocation turn2)
: DrivetrainInputReader(wheel_high, throttle_high, quick_turn, turn1,
- turn2),
+ TurnButtonUse::kLineFollow, turn2,
+ TurnButtonUse::kLineFollow),
wheel_low_(wheel_low),
wheel_velocity_high_(wheel_velocity_high),
wheel_velocity_low_(wheel_velocity_low),
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 0f57bf9..53c2315 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -92,6 +92,9 @@
Scalar wheel_multiplier;
+ // Whether the shift button on the pistol grip enables line following mode.
+ bool pistol_grip_shift_enables_line_follow = false;
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
diff --git a/y2019/control_loops/drivetrain/drivetrain_base.cc b/y2019/control_loops/drivetrain/drivetrain_base.cc
index 02bd805..1dfe99a 100644
--- a/y2019/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_base.cc
@@ -28,20 +28,31 @@
::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
::frc971::control_loops::drivetrain::IMUType::IMU_X,
- drivetrain::MakeDrivetrainLoop, drivetrain::MakeVelocityDrivetrainLoop,
+ drivetrain::MakeDrivetrainLoop,
+ drivetrain::MakeVelocityDrivetrainLoop,
drivetrain::MakeKFDrivetrainLoop,
drivetrain::MakeHybridVelocityDrivetrainLoop,
chrono::duration_cast<chrono::nanoseconds>(
chrono::duration<double>(drivetrain::kDt)),
- drivetrain::kRobotRadius, drivetrain::kWheelRadius, drivetrain::kV,
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kV,
- drivetrain::kHighGearRatio, drivetrain::kLowGearRatio, drivetrain::kJ,
- drivetrain::kMass, kThreeStateDriveShifter, kThreeStateDriveShifter,
- true /* default_high_gear */, 0 /* down_offset if using constants use
- constants::GetValues().down_error */,
- 0.7 /* wheel_non_linearity */, 1.2 /* quickturn_wheel_multiplier */,
+ drivetrain::kHighGearRatio,
+ drivetrain::kLowGearRatio,
+ drivetrain::kJ,
+ drivetrain::kMass,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ true /* default_high_gear */,
+ 0 /* down_offset if using constants use
+ constants::GetValues().down_error */
+ ,
+ 0.7 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */,
1.2 /* wheel_multiplier */,
+ true /*pistol_grip_shift_enables_line_follow*/,
};
return kDrivetrainConfig;