Made pistol grip controller work with the drivetrain.
Change-Id: Id5ede34e9a9981c338aaa6118eb5085578a8ecd3
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 1f6d713..c4fd232 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -27,7 +27,11 @@
const auto wheel_and_throttle = GetWheelAndThrottle(data);
const double wheel = wheel_and_throttle.wheel;
+ const double wheel_velocity = wheel_and_throttle.wheel_velocity;
+ const double wheel_torque = wheel_and_throttle.wheel_torque;
const double throttle = wheel_and_throttle.throttle;
+ const double throttle_velocity = wheel_and_throttle.throttle_velocity;
+ const double throttle_torque = wheel_and_throttle.throttle_torque;
const bool high_gear = wheel_and_throttle.high_gear;
drivetrain_queue.status.FetchLatest();
@@ -49,8 +53,12 @@
is_control_loop_driving = true;
}
auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
- new_drivetrain_goal->steering = wheel;
+ new_drivetrain_goal->wheel = wheel;
+ new_drivetrain_goal->wheel_velocity = wheel_velocity;
+ new_drivetrain_goal->wheel_torque = wheel_torque;
new_drivetrain_goal->throttle = throttle;
+ new_drivetrain_goal->throttle_velocity = throttle_velocity;
+ new_drivetrain_goal->throttle_torque = throttle_torque;
new_drivetrain_goal->highgear = high_gear;
new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
@@ -70,8 +78,8 @@
DrivetrainInputReader::WheelAndThrottle
SteeringWheelDrivetrainInputReader::GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) {
- const double wheel = -data.GetAxis(steering_wheel_);
- const double throttle = -data.GetAxis(drive_throttle_);
+ const double wheel = -data.GetAxis(wheel_);
+ const double throttle = -data.GetAxis(throttle_);
if (!data.GetControlBit(ControlBit::kEnabled)) {
high_gear_ = default_high_gear_;
@@ -85,37 +93,70 @@
high_gear_ = true;
}
- return DrivetrainInputReader::WheelAndThrottle{wheel, throttle, high_gear_};
+ return DrivetrainInputReader::WheelAndThrottle{
+ wheel, 0.0, 0.0, throttle, 0.0, 0.0, high_gear_};
+}
+
+double UnwrappedAxis(const ::aos::input::driver_station::Data &data,
+ const JoystickAxis &high_bits,
+ const JoystickAxis &low_bits) {
+ const float high_bits_data = data.GetAxis(high_bits);
+ const float low_bits_data = data.GetAxis(low_bits);
+ const int16_t high_bits_data_int =
+ (high_bits_data < 0.0f ? high_bits_data * 128.0f
+ : high_bits_data * 127.0f);
+ const int16_t low_bits_data_int =
+ (low_bits_data < 0.0f ? low_bits_data * 128.0f : low_bits_data * 127.0f);
+
+ const uint16_t high_bits_data_uint =
+ ((static_cast<uint16_t>(high_bits_data_int) & 0xff) + 0x80) & 0xff;
+ const uint16_t low_bits_data_uint =
+ ((static_cast<uint16_t>(low_bits_data_int) & 0xff) + 0x80) & 0xff;
+
+ const uint16_t data_uint = (high_bits_data_uint << 8) | low_bits_data_uint;
+
+ const int32_t data_int = static_cast<int32_t>(data_uint) - 0x8000;
+
+ if (data_int < 0) {
+ return static_cast<double>(data_int) / static_cast<double>(0x8000);
+ } else {
+ return static_cast<double>(data_int) / static_cast<double>(0x7fff);
+ }
}
DrivetrainInputReader::WheelAndThrottle
PistolDrivetrainInputReader::GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) {
- const double unscaled_wheel = data.GetAxis(steering_wheel_);
- double wheel;
- if (unscaled_wheel < 0.0) {
- wheel = unscaled_wheel / 0.484375;
- } else {
- wheel = unscaled_wheel / 0.385827;
+ const double wheel =
+ -UnwrappedAxis(data, wheel_, wheel_low_);
+ const double wheel_velocity =
+ -UnwrappedAxis(data, wheel_velocity_high_, wheel_velocity_low_) * 50.0;
+ const double wheel_torque =
+ -UnwrappedAxis(data, wheel_torque_high_, wheel_torque_low_) / 2.0;
+
+ const double throttle =
+ UnwrappedAxis(data, throttle_, throttle_low_);
+ const double throttle_velocity =
+ UnwrappedAxis(data, throttle_velocity_high_, throttle_velocity_low_) * 50.0;
+ const double throttle_torque =
+ UnwrappedAxis(data, throttle_torque_high_, throttle_torque_low_) / 2.0;
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ high_gear_ = default_high_gear_;
}
- const double unscaled_throttle = -data.GetAxis(drive_throttle_);
- double unmodified_throttle;
- if (unscaled_throttle < 0.0) {
- unmodified_throttle = unscaled_throttle / 0.086614;
- } else {
- unmodified_throttle = unscaled_throttle / 0.265625;
+ if (data.PosEdge(shift_low_)) {
+ high_gear_ = false;
}
- unmodified_throttle = aos::Deadband(unmodified_throttle, 0.1, 1.0);
- // Apply a sin function that's scaled to make it feel better.
- constexpr double throttle_range = M_PI_2 * 0.5;
+ if (data.PosEdge(shift_high_)) {
+ high_gear_ = true;
+ }
- double throttle = ::std::sin(throttle_range * unmodified_throttle) /
- ::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, true};
+ return DrivetrainInputReader::WheelAndThrottle{
+ wheel, wheel_velocity, wheel_torque,
+ throttle, throttle_velocity, throttle_torque,
+ high_gear_};
}
DrivetrainInputReader::WheelAndThrottle
@@ -125,10 +166,10 @@
constexpr double kWheelDeadband = 0.05;
constexpr double kThrottleDeadband = 0.05;
const double wheel =
- aos::Deadband(-data.GetAxis(steering_wheel_), kWheelDeadband, 1.0);
+ aos::Deadband(-data.GetAxis(wheel_), kWheelDeadband, 1.0);
const double unmodified_throttle =
- aos::Deadband(-data.GetAxis(drive_throttle_), kThrottleDeadband, 1.0);
+ aos::Deadband(-data.GetAxis(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;
@@ -137,7 +178,8 @@
::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, true};
+ return DrivetrainInputReader::WheelAndThrottle{wheel, 0.0, 0.0, throttle,
+ 0.0, 0.0, true};
}
std::unique_ptr<SteeringWheelDrivetrainInputReader>
@@ -154,19 +196,32 @@
return result;
}
-std::unique_ptr<PistolDrivetrainInputReader>
-PistolDrivetrainInputReader::Make() {
+std::unique_ptr<PistolDrivetrainInputReader> PistolDrivetrainInputReader::Make(
+ bool default_high_gear) {
// Pistol Grip controller
- const JoystickAxis kSteeringWheel(1, 2), kDriveThrottle(1, 1);
- const ButtonLocation kQuickTurn(1, 7);
- const ButtonLocation kTurn1(1, 8);
+ const JoystickAxis kTriggerHigh(1, 1), kTriggerLow(1, 4),
+ kTriggerVelocityHigh(1, 2), kTriggerVelocityLow(1, 5),
+ kTriggerTorqueHigh(1, 3), kTriggerTorqueLow(1, 6);
+
+ const JoystickAxis kWheelHigh(2, 1), kWheelLow(2, 4),
+ kWheelVelocityHigh(2, 2), kWheelVelocityLow(2, 5), kWheelTorqueHigh(2, 3),
+ kWheelTorqueLow(2, 6);
+
+ const ButtonLocation kQuickTurn(1, 4);
+ const ButtonLocation kShiftHigh(1, 2);
+ const ButtonLocation kShiftLow(1, 3);
// Nop
- const ButtonLocation kTurn2(1, 9);
+ const ButtonLocation kTurn1(1, 9);
+ const ButtonLocation kTurn2(1, 10);
std::unique_ptr<PistolDrivetrainInputReader> result(
- new PistolDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
- kQuickTurn, kTurn1, kTurn2));
- result->set_wheel_multiplier(0.2);
+ new PistolDrivetrainInputReader(
+ kWheelHigh, kWheelLow, kTriggerVelocityHigh, kTriggerVelocityLow,
+ kTriggerTorqueHigh, kTriggerTorqueLow, kTriggerHigh, kTriggerLow,
+ kWheelVelocityHigh, kWheelVelocityLow, kWheelTorqueHigh,
+ kWheelTorqueLow, kQuickTurn, kShiftHigh, kShiftLow, kTurn1, kTurn2));
+
+ result->set_default_high_gear(default_high_gear);
return result;
}
@@ -197,7 +252,8 @@
SteeringWheelDrivetrainInputReader::Make(dt_config.default_high_gear);
break;
case InputType::kPistol:
- drivetrain_input_reader = PistolDrivetrainInputReader::Make();
+ drivetrain_input_reader =
+ PistolDrivetrainInputReader::Make(dt_config.default_high_gear);
break;
case InputType::kXbox:
drivetrain_input_reader = XboxDrivetrainInputReader::Make();
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 062332d..86b0e10 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -36,13 +36,13 @@
class DrivetrainInputReader {
public:
// Inputs driver station button and joystick locations
- DrivetrainInputReader(driver_station::JoystickAxis steering_wheel,
- driver_station::JoystickAxis drive_throttle,
+ DrivetrainInputReader(driver_station::JoystickAxis wheel,
+ driver_station::JoystickAxis throttle,
driver_station::ButtonLocation quick_turn,
driver_station::ButtonLocation turn1,
driver_station::ButtonLocation turn2)
- : steering_wheel_(steering_wheel),
- drive_throttle_(drive_throttle),
+ : wheel_(wheel),
+ throttle_(throttle),
quick_turn_(quick_turn),
turn1_(turn1),
turn2_(turn2) {}
@@ -74,8 +74,8 @@
double robot_velocity() const { return robot_velocity_; }
protected:
- const driver_station::JoystickAxis steering_wheel_;
- const driver_station::JoystickAxis drive_throttle_;
+ const driver_station::JoystickAxis wheel_;
+ const driver_station::JoystickAxis throttle_;
const driver_station::ButtonLocation quick_turn_;
const driver_station::ButtonLocation turn1_;
const driver_station::ButtonLocation turn2_;
@@ -84,7 +84,11 @@
// values from the joysticks.
struct WheelAndThrottle {
double wheel;
+ double wheel_velocity;
+ double wheel_torque;
double throttle;
+ double throttle_velocity;
+ double throttle_torque;
bool high_gear;
};
@@ -131,11 +135,68 @@
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the (cheap) pistol grip controller.
- static std::unique_ptr<PistolDrivetrainInputReader> Make();
+ static std::unique_ptr<PistolDrivetrainInputReader> Make(bool default_high_gear);
private:
+ PistolDrivetrainInputReader(
+ driver_station::JoystickAxis wheel_high,
+ driver_station::JoystickAxis wheel_low,
+ driver_station::JoystickAxis wheel_velocity_high,
+ driver_station::JoystickAxis wheel_velocity_low,
+ driver_station::JoystickAxis wheel_torque_high,
+ driver_station::JoystickAxis wheel_torque_low,
+ driver_station::JoystickAxis throttle_high,
+ driver_station::JoystickAxis throttle_low,
+ driver_station::JoystickAxis throttle_velocity_high,
+ driver_station::JoystickAxis throttle_velocity_low,
+ driver_station::JoystickAxis throttle_torque_high,
+ driver_station::JoystickAxis throttle_torque_low,
+ driver_station::ButtonLocation quick_turn,
+ driver_station::ButtonLocation shift_high,
+ driver_station::ButtonLocation shift_low,
+ driver_station::ButtonLocation turn1,
+ driver_station::ButtonLocation turn2)
+ : DrivetrainInputReader(wheel_high, throttle_high, quick_turn, turn1,
+ turn2),
+ wheel_low_(wheel_low),
+ wheel_velocity_high_(wheel_velocity_high),
+ wheel_velocity_low_(wheel_velocity_low),
+ wheel_torque_high_(wheel_torque_high),
+ wheel_torque_low_(wheel_torque_low),
+ throttle_low_(throttle_low),
+ throttle_velocity_high_(throttle_velocity_high),
+ throttle_velocity_low_(throttle_velocity_low),
+ throttle_torque_high_(throttle_torque_high),
+ throttle_torque_low_(throttle_torque_low),
+ shift_high_(shift_high),
+ shift_low_(shift_low) {}
+
WheelAndThrottle GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) override;
+
+ // 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;
+ }
+
+ const driver_station::JoystickAxis wheel_low_;
+ const driver_station::JoystickAxis wheel_velocity_high_;
+ const driver_station::JoystickAxis wheel_velocity_low_;
+ const driver_station::JoystickAxis wheel_torque_high_;
+ const driver_station::JoystickAxis wheel_torque_low_;
+
+ const driver_station::JoystickAxis throttle_low_;
+ const driver_station::JoystickAxis throttle_velocity_high_;
+ const driver_station::JoystickAxis throttle_velocity_low_;
+ const driver_station::JoystickAxis throttle_torque_high_;
+ const driver_station::JoystickAxis throttle_torque_low_;
+
+ const driver_station::ButtonLocation shift_high_;
+ const driver_station::ButtonLocation shift_low_;
+
+ bool high_gear_;
+ bool default_high_gear_;
};
class XboxDrivetrainInputReader : public DrivetrainInputReader {
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 5311810..abb91a7 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -30,7 +30,7 @@
drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
.highgear(true)
- .steering(0.0)
+ .wheel(0.0)
.throttle(0.0)
.left_goal(initial_drivetrain_.left)
.left_velocity_goal(0)
@@ -58,7 +58,7 @@
auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
drivetrain_message->control_loop_driving = true;
drivetrain_message->highgear = true;
- drivetrain_message->steering = 0.0;
+ drivetrain_message->wheel = 0.0;
drivetrain_message->throttle = 0.0;
drivetrain_message->left_goal = initial_drivetrain_.left;
drivetrain_message->left_velocity_goal = 0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 76f5ec6..a756ad2 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -40,10 +40,14 @@
message Goal {
// Position of the steering wheel (positive = turning left when going
// forwards).
- double steering;
+ double wheel;
+ double wheel_velocity;
+ double wheel_torque;
// Position of the throttle (positive forwards).
double throttle;
+ double throttle_velocity;
+ double throttle_torque;
// True to shift into high, false to shift into low.
bool highgear;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 5cc15d1..594d504 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -468,7 +468,7 @@
TEST_F(DrivetrainTest, OpenLoopThenClosed) {
my_drivetrain_queue_.goal.MakeWithBuilder()
.control_loop_driving(false)
- .steering(0.0)
+ .wheel(0.0)
.throttle(1.0)
.highgear(true)
.quickturn(false)
@@ -478,7 +478,7 @@
my_drivetrain_queue_.goal.MakeWithBuilder()
.control_loop_driving(false)
- .steering(0.0)
+ .wheel(0.0)
.throttle(-0.3)
.highgear(true)
.quickturn(false)
@@ -488,7 +488,7 @@
my_drivetrain_queue_.goal.MakeWithBuilder()
.control_loop_driving(false)
- .steering(0.0)
+ .wheel(0.0)
.throttle(0.0)
.highgear(true)
.quickturn(false)
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index f61083b..a842f50 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -108,7 +108,7 @@
void PolyDrivetrain::SetGoal(
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- const double wheel = goal.steering;
+ const double wheel = goal.wheel;
const double throttle = goal.throttle;
const bool quickturn = goal.quickturn;
const bool highgear = goal.highgear;
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index 24c1b6c..3b5bc48 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -100,7 +100,7 @@
is_high_gear_ = true;
}
if (!drivetrain_queue.goal.MakeWithBuilder()
- .steering(wheel)
+ .wheel(wheel)
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index e443f6b..96c7132 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -67,7 +67,7 @@
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
.highgear(true)
- .steering(0.0)
+ .wheel(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
.left_velocity_goal(0)
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 2bef2f7..7516c97 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -225,7 +225,7 @@
LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
}
if (!drivetrain_queue.goal.MakeWithBuilder()
- .steering(wheel)
+ .wheel(wheel)
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
diff --git a/y2014_bot3/autonomous/auto.cc b/y2014_bot3/autonomous/auto.cc
index 90d8457..6b63fab 100644
--- a/y2014_bot3/autonomous/auto.cc
+++ b/y2014_bot3/autonomous/auto.cc
@@ -36,7 +36,7 @@
LOG(INFO, "resetting the drivetrain\n");
::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
- .steering(0.0)
+ .wheel(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
.left_velocity_goal(0)
@@ -68,7 +68,7 @@
.control_loop_driving(false)
.highgear(false)
.quickturn(false)
- .steering(0.0)
+ .wheel(0.0)
.throttle(0.5)
.left_goal(left_initial_position)
.left_velocity_goal(0)
@@ -81,7 +81,7 @@
.control_loop_driving(false)
.highgear(false)
.quickturn(false)
- .steering(0.0)
+ .wheel(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
.left_velocity_goal(0)
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index a29856b..b4126d1 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -81,7 +81,7 @@
}
if (!drivetrain_queue.goal.MakeWithBuilder()
- .steering(wheel)
+ .wheel(wheel)
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 9491cd4..0c9a2d9 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -60,7 +60,7 @@
const double right_current = vision_status.drivetrain_right_position;
if (!drivetrain_queue.goal.MakeWithBuilder()
- .steering(0.0)
+ .wheel(0.0)
.throttle(0.0)
.highgear(false)
.quickturn(false)
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 35477be..0cff7fb 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -159,7 +159,7 @@
is_control_loop_driving = true;
}
if (!drivetrain_queue.goal.MakeWithBuilder()
- .steering(wheel)
+ .wheel(wheel)
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 9286163..2179606 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -50,7 +50,7 @@
public:
Reader() {
drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kSteeringWheel,
+ DrivetrainInputReader::InputType::kPistol,
::y2018::control_loops::drivetrain::GetDrivetrainConfig());
}