Change leg length when legged_balance chassis gyro.

rm-controls · Nov 1, 2024 · b70a509 · b70a509
1 parent ac4de85
commit b70a509
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 2 deletions.
diff --git 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
@@ -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;
   }
 }