Third robot bringup adjustments
Change-Id: I48d28520d4b7e068029b5a0fbb34ca93dbdc85d6
diff --git a/aos/input/BUILD b/aos/input/BUILD
index d8fc2c8..ad1915b 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -33,5 +33,6 @@
'//aos/common/network:socket',
'//aos/common:math',
'//frc971/control_loops/drivetrain:drivetrain_queue',
+ '//frc971/control_loops/drivetrain:drivetrain_config',
],
)
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index a0d95cf..1f6d713 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -19,6 +19,8 @@
namespace aos {
namespace input {
+const ButtonLocation kShiftHigh(2, 3), kShiftHigh2(2, 2), kShiftLow(2, 1);
+
void DrivetrainInputReader::HandleDrivetrain(
const ::aos::input::driver_station::Data &data) {
bool is_control_loop_driving = false;
@@ -26,13 +28,14 @@
const auto wheel_and_throttle = GetWheelAndThrottle(data);
const double wheel = wheel_and_throttle.wheel;
const double throttle = wheel_and_throttle.throttle;
+ const bool high_gear = wheel_and_throttle.high_gear;
drivetrain_queue.status.FetchLatest();
if (drivetrain_queue.status.get()) {
robot_velocity_ = drivetrain_queue.status->robot_speed;
}
- if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
+ if (data.PosEdge(turn1_) || data.PosEdge(turn2_)) {
if (drivetrain_queue.status.get()) {
left_goal_ = drivetrain_queue.status->estimated_left_position;
right_goal_ = drivetrain_queue.status->estimated_right_position;
@@ -42,13 +45,14 @@
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
- if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
+ if (data.IsPressed(turn1_) || data.IsPressed(turn2_)) {
is_control_loop_driving = true;
}
auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
new_drivetrain_goal->steering = wheel;
new_drivetrain_goal->throttle = throttle;
- new_drivetrain_goal->quickturn = data.IsPressed(kQuickTurn);
+ new_drivetrain_goal->highgear = high_gear;
+ new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
new_drivetrain_goal->left_goal = current_left_goal;
new_drivetrain_goal->right_goal = current_right_goal;
@@ -66,15 +70,28 @@
DrivetrainInputReader::WheelAndThrottle
SteeringWheelDrivetrainInputReader::GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) {
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
- return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+ const double wheel = -data.GetAxis(steering_wheel_);
+ const double throttle = -data.GetAxis(drive_throttle_);
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ high_gear_ = default_high_gear_;
+ }
+
+ if (data.PosEdge(kShiftLow)) {
+ high_gear_ = false;
+ }
+
+ if (data.PosEdge(kShiftHigh) || data.PosEdge(kShiftHigh2)) {
+ high_gear_ = true;
+ }
+
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle, high_gear_};
}
DrivetrainInputReader::WheelAndThrottle
PistolDrivetrainInputReader::GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) {
- const double unscaled_wheel = data.GetAxis(kSteeringWheel);
+ const double unscaled_wheel = data.GetAxis(steering_wheel_);
double wheel;
if (unscaled_wheel < 0.0) {
wheel = unscaled_wheel / 0.484375;
@@ -82,7 +99,7 @@
wheel = unscaled_wheel / 0.385827;
}
- const double unscaled_throttle = -data.GetAxis(kDriveThrottle);
+ const double unscaled_throttle = -data.GetAxis(drive_throttle_);
double unmodified_throttle;
if (unscaled_throttle < 0.0) {
unmodified_throttle = unscaled_throttle / 0.086614;
@@ -98,7 +115,7 @@
::std::sin(throttle_range);
throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
throttle = 2.0 * unmodified_throttle - throttle;
- return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle, true};
}
DrivetrainInputReader::WheelAndThrottle
@@ -108,10 +125,10 @@
constexpr double kWheelDeadband = 0.05;
constexpr double kThrottleDeadband = 0.05;
const double wheel =
- aos::Deadband(-data.GetAxis(kSteeringWheel), kWheelDeadband, 1.0);
+ aos::Deadband(-data.GetAxis(steering_wheel_), kWheelDeadband, 1.0);
const double unmodified_throttle =
- aos::Deadband(-data.GetAxis(kDriveThrottle), kThrottleDeadband, 1.0);
+ aos::Deadband(-data.GetAxis(drive_throttle_), kThrottleDeadband, 1.0);
// Apply a sin function that's scaled to make it feel better.
constexpr double throttle_range = M_PI_2 * 0.9;
@@ -120,11 +137,11 @@
::std::sin(throttle_range);
throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
throttle = 2.0 * unmodified_throttle - throttle;
- return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle, true};
}
std::unique_ptr<SteeringWheelDrivetrainInputReader>
-SteeringWheelDrivetrainInputReader::Make() {
+SteeringWheelDrivetrainInputReader::Make(bool default_high_gear) {
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kQuickTurn(1, 5);
const ButtonLocation kTurn1(1, 7);
@@ -132,6 +149,8 @@
std::unique_ptr<SteeringWheelDrivetrainInputReader> result(
new SteeringWheelDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
kQuickTurn, kTurn1, kTurn2));
+ result.get()->set_default_high_gear(default_high_gear);
+
return result;
}
@@ -166,13 +185,16 @@
return result;
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
- InputType type) {
+ InputType type,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig &dt_config) {
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
using InputType = DrivetrainInputReader::InputType;
+
switch (type) {
case InputType::kSteeringWheel:
- drivetrain_input_reader = SteeringWheelDrivetrainInputReader::Make();
+ drivetrain_input_reader =
+ SteeringWheelDrivetrainInputReader::Make(dt_config.default_high_gear);
break;
case InputType::kPistol:
drivetrain_input_reader = PistolDrivetrainInputReader::Make();
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 4b13ac6..062332d 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -10,6 +10,7 @@
#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
namespace aos {
namespace input {
@@ -35,16 +36,16 @@
class DrivetrainInputReader {
public:
// Inputs driver station button and joystick locations
- DrivetrainInputReader(driver_station::JoystickAxis kSteeringWheel,
- driver_station::JoystickAxis kDriveThrottle,
- driver_station::ButtonLocation kQuickTurn,
- driver_station::ButtonLocation kTurn1,
- driver_station::ButtonLocation kTurn2)
- : kSteeringWheel(kSteeringWheel),
- kDriveThrottle(kDriveThrottle),
- kQuickTurn(kQuickTurn),
- kTurn1(kTurn1),
- kTurn2(kTurn2) {}
+ DrivetrainInputReader(driver_station::JoystickAxis steering_wheel,
+ driver_station::JoystickAxis drive_throttle,
+ driver_station::ButtonLocation quick_turn,
+ driver_station::ButtonLocation turn1,
+ driver_station::ButtonLocation turn2)
+ : steering_wheel_(steering_wheel),
+ drive_throttle_(drive_throttle),
+ quick_turn_(quick_turn),
+ turn1_(turn1),
+ turn2_(turn2) {}
virtual ~DrivetrainInputReader() = default;
@@ -56,7 +57,9 @@
};
// Constructs the appropriate DrivetrainInputReader.
- static std::unique_ptr<DrivetrainInputReader> Make(InputType type);
+ static std::unique_ptr<DrivetrainInputReader> Make(
+ InputType type,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig &dt_config);
// Processes new joystick data and publishes drivetrain goal messages.
void HandleDrivetrain(const ::aos::input::driver_station::Data &data);
@@ -71,17 +74,18 @@
double robot_velocity() const { return robot_velocity_; }
protected:
- const driver_station::JoystickAxis kSteeringWheel;
- const driver_station::JoystickAxis kDriveThrottle;
- const driver_station::ButtonLocation kQuickTurn;
- const driver_station::ButtonLocation kTurn1;
- const driver_station::ButtonLocation kTurn2;
+ const driver_station::JoystickAxis steering_wheel_;
+ const driver_station::JoystickAxis drive_throttle_;
+ const driver_station::ButtonLocation quick_turn_;
+ const driver_station::ButtonLocation turn1_;
+ const driver_station::ButtonLocation turn2_;
// Structure containing the (potentially adjusted) steering and throttle
// values from the joysticks.
struct WheelAndThrottle {
double wheel;
double throttle;
+ bool high_gear;
};
private:
@@ -105,11 +109,20 @@
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the big steering wheel and throttle stick.
- static std::unique_ptr<SteeringWheelDrivetrainInputReader> Make();
+ static std::unique_ptr<SteeringWheelDrivetrainInputReader> Make(
+ bool default_high_gear);
+
+ // Sets the default shifter position
+ void set_default_high_gear(bool default_high_gear) {
+ high_gear_ = default_high_gear;
+ default_high_gear_ = default_high_gear;
+ }
private:
WheelAndThrottle GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) override;
+ bool high_gear_;
+ bool default_high_gear_;
};
class PistolDrivetrainInputReader : public DrivetrainInputReader {