Merge pull request #281 from 321Aurora/fixed-for-wheel-speed

Achieve fixed front friction wheel speed for hero

Author: YoujianWu

rm_common/include/rm_common/decision/command_sender.h

@@ -381,6 +381,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
     nh.getParam("wheel_speed_16", wheel_speed_16_);
     nh.getParam("wheel_speed_18", wheel_speed_18_);
     nh.getParam("wheel_speed_30", wheel_speed_30_);
+    nh.param("wheel_speed_offset_front", wheel_speed_offset_front_, 0.0);
+    nh.param("wheel_speed_offset_back", wheel_speed_offset_back_, 0.0);
     nh.param("speed_oscillation", speed_oscillation_, 1.0);
     nh.param("extra_wheel_speed_once", extra_wheel_speed_once_, 0.);
     nh.param("deploy_wheel_speed", deploy_wheel_speed_, 410.0);
@@ -484,6 +486,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   void sendCommand(const ros::Time& time) override
   {
     msg_.wheel_speed = getWheelSpeedDes();
+    msg_.wheels_speed_offset_front = getFrontWheelSpeedOffset();
+    msg_.wheels_speed_offset_back = getBackWheelSpeedOffset();
     msg_.hz = heat_limit_->getShootFrequency();
     TimeStampCommandSenderBase<rm_msgs::ShootCmd>::sendCommand(time);
   }
@@ -495,16 +499,22 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   double getWheelSpeedDes()
   {
     setSpeedDesAndWheelSpeedDes();
-    if (deploy_flag_)
+    if (hero_flag_)
     {
-      return deploy_wheel_speed_ + total_extra_wheel_speed_;
+      if (deploy_flag_)
+        return deploy_wheel_speed_;
+      return wheel_speed_des_;
     }
     return wheel_speed_des_ + total_extra_wheel_speed_;
   }
   void setDeployState(bool flag)
   {
     deploy_flag_ = flag;
   }
+  void setHeroState(bool flag)
+  {
+    hero_flag_ = flag;
+  }
   bool getDeployState()
   {
     return deploy_flag_;
@@ -550,13 +560,29 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
       }
     }
   }
+  double getFrontWheelSpeedOffset()
+  {
+    wheels_speed_offset_front_ = wheel_speed_offset_front_;
+    return wheels_speed_offset_front_;
+  }
+  double getBackWheelSpeedOffset()
+  {
+    wheels_speed_offset_back_ = wheel_speed_offset_back_;
+    return wheels_speed_offset_back_;
+  }
   void dropSpeed()
   {
-    total_extra_wheel_speed_ -= extra_wheel_speed_once_;
+    if (hero_flag_)
+      wheel_speed_offset_front_ -= extra_wheel_speed_once_;
+    else
+      total_extra_wheel_speed_ -= extra_wheel_speed_once_;
   }
   void raiseSpeed()
   {
-    total_extra_wheel_speed_ += extra_wheel_speed_once_;
+    if (hero_flag_)
+      wheel_speed_offset_front_ += extra_wheel_speed_once_;
+    else
+      total_extra_wheel_speed_ += extra_wheel_speed_once_;
   }
   void setArmorType(uint8_t armor_type)
   {
@@ -577,6 +603,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{}, speed_limit_{};
   double wheel_speed_10_{}, wheel_speed_15_{}, wheel_speed_16_{}, wheel_speed_18_{}, wheel_speed_30_{},
       wheel_speed_des_{}, last_bullet_speed_{}, speed_oscillation_{};
+  double wheel_speed_offset_front_{}, wheel_speed_offset_back_{};
+  double wheels_speed_offset_front_{}, wheels_speed_offset_back_{};
   double track_armor_error_tolerance_{};
   double track_buff_error_tolerance_{};
   double untrack_armor_error_tolerance_{};
@@ -586,6 +614,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   double total_extra_wheel_speed_{};
   double deploy_wheel_speed_{};
   bool auto_wheel_speed_ = false;
+  bool hero_flag_{};
   bool deploy_flag_ = false;
   rm_msgs::TrackData track_data_;
   rm_msgs::GimbalDesError gimbal_des_error_;

rm_msgs/msg/ShootCmd.msg

@@ -12,4 +12,6 @@ uint8 SPEED_30M_PER_SECOND = 5
 uint8 mode
 float64 wheel_speed
 float64 hz
+float64 wheels_speed_offset_front
+float64 wheels_speed_offset_back
 time stamp