Add a centering knotch to the joystick.
Change-Id: Ic6bbfd9151073c495e88bb55bf2f4ac8c25730f5
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index a5827c3..506e4b6 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -134,13 +134,18 @@
const double wheel_torque =
-UnwrappedAxis(data, wheel_torque_high_, wheel_torque_low_) / 2.0;
- const double throttle =
+ 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;
+ // TODO(austin): Deal with haptics here.
+ if (throttle < 0) {
+ throttle = ::std::max(-1.0, throttle / 0.7);
+ }
+
if (!data.GetControlBit(ControlBit::kEnabled)) {
high_gear_ = default_high_gear_;
}
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index 2bf7462..6e1c710 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -156,18 +156,28 @@
->HandleInterrupt(BalanceSimpleReadings(readings.currents), encoder);
}
-constexpr float kTriggerMaxExtension = -1.00f;
+constexpr float kTriggerMaxExtension = -0.70f;
constexpr float kTriggerCenter = 0.0f;
+constexpr float kCenteringStiffness = 0.15f;
float TriggerCenteringCurrent(float trigger_angle) {
float goal_current = (kTriggerCenter - trigger_angle) * 3.0f;
+ float knotch_goal_current = (kTriggerCenter - trigger_angle) * 8.0f;
+ if (knotch_goal_current < -kCenteringStiffness) {
+ knotch_goal_current = -kCenteringStiffness;
+ } else if (knotch_goal_current > kCenteringStiffness) {
+ knotch_goal_current = kCenteringStiffness;
+ }
+
+ goal_current += knotch_goal_current;
+
if (goal_current < -1.0f) {
goal_current = -1.0f;
} else if (goal_current > 1.0f) {
goal_current = 1.0f;
if (trigger_angle < kTriggerMaxExtension) {
goal_current -= (30.0f * (trigger_angle - kTriggerMaxExtension));
- if (goal_current > 2.0f) {
- goal_current = 2.0f;
+ if (goal_current > 4.0f) {
+ goal_current = 4.0f;
}
}
}