diff --git a/include/rm_manual/legged_wheel_balance_manual.h b/include/rm_manual/legged_wheel_balance_manual.h index f3872d16..43d83029 100644 --- a/include/rm_manual/legged_wheel_balance_manual.h +++ b/include/rm_manual/legged_wheel_balance_manual.h @@ -31,7 +31,7 @@ class LeggedWheelBalanceManual : public BalanceManual rm_common::LegCommandSender* legCommandSender_{}; private: - bool stretch_ = false, stretching_ = false; + bool stretch_ = false, stretching_ = false, is_increasing_length_ = false; InputEvent ctrl_shift_event_, ctrl_g_event_; ros::Subscriber unstick_sub_; void unstickCallback(const std_msgs::BoolConstPtr& msg); diff --git a/src/legged_wheel_balance_manual.cpp b/src/legged_wheel_balance_manual.cpp index 44563c7d..976b3677 100644 --- a/src/legged_wheel_balance_manual.cpp +++ b/src/legged_wheel_balance_manual.cpp @@ -30,6 +30,30 @@ LeggedWheelBalanceManual::LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::Nod void LeggedWheelBalanceManual::sendCommand(const ros::Time& time) { BalanceManual::sendCommand(time); + if (is_gyro_) + { + double current_length = legCommandSender_->getLgeLength(); + if (is_increasing_length_) + { + if (current_length < 0.3) + { + double delta = current_length + 0.002; + legCommandSender_->setLgeLength(delta > 0.3 ? 0.3 : delta); + } + else + is_increasing_length_ = false; + } + else + { + if (current_length > 0.18) + { + double delta = current_length - 0.002; + legCommandSender_->setLgeLength(delta < 0.18 ? 0.18 : delta); + } + else + is_increasing_length_ = true; + } + } legCommandSender_->sendCommand(time); } @@ -135,7 +159,7 @@ void LeggedWheelBalanceManual::unstickCallback(const std_msgs::BoolConstPtr& msg } else if (stretching_ && !two_leg_unstick) { - legCommandSender_->setLgeLength(0.2); + legCommandSender_->setLgeLength(0.18); stretching_ = false; } }