增加原地调整斜坡

MPC/navigation.cpp

@@ -37,6 +37,19 @@ double limit(double x,double min,double max){
 
 }
 
+double next_speed(double speed,double target_speed,double acc,double dt=0.1)
+{
+  if(fabs(target_speed-speed)/dt<acc)
+  {
+    return target_speed;
+  }
+  else
+  {
+    double r=target_speed-speed>=0.?1.0:-1.0;
+    return speed+r*acc*dt;
+  }
+}
+
 
 
 bool Navigation::Init(const Navigation_parameter& parameter)
@@ -297,6 +310,8 @@ void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
   navigator->HandleNavCmd(cmd);
 }
 
+
+
 bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,
                                     stLimit limit_v,stLimit limit_h,stLimit limit_rotate)
 {
@@ -345,26 +360,34 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
     }
     else
     {
+      double acc_v=2.0;
+      double acc_angular=20.0*M_PI/180.0;
+      double dt=0.1;
       if (action == 1) {
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
-          double theta = limit(diff.theta(),limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
-          SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, theta);
+          double theta = (diff.theta(),limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
+          double angular=next_speed(timedA_.Get(),theta,acc_angular,dt);
+          SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, angular);
           //std::cout<<"current:"<<timedPose_.Get()<<"  target:"<<target<<" diff:"<<diff<<std::endl;
-          printf(" Ratate :%f\n",theta);
+          printf(" Ratate angular:%f,target:%f\n",angular,theta);
           continue;
         }
       }
       if (action == 2) {
         if (Pose2d::abs(diff).y() > thresh.y()) {
-          SendMoveCmd(move_mode_,Monitor_emqx::eHorizontal, limit(diff.y(),limit_h.min,limit_h.max), 0);
-          printf(" Horizontal :%f\n",limit(diff.y(),limit_h.min,limit_h.max));
+          double hv=limit(diff.y(),limit_h.min,limit_h.max);
+          double velocity=next_speed(timedA_.Get(),hv,acc_v,dt);
+          SendMoveCmd(move_mode_,Monitor_emqx::eHorizontal,velocity , 0);
+          printf(" Horizontal :%f\n",velocity);
           continue;
         }
       }
       if (action == 3) {
         if (Pose2d::abs(diff).x() > thresh.x()) {
-          SendMoveCmd(move_mode_,Monitor_emqx::eMPC, limit(diff.x(),limit_v.min,limit_v.max), 0);
-          printf(" Vrtical :%f\n",limit(diff.x(),limit_v.min,limit_v.max));
+          double v=limit(diff.x(),limit_v.min,limit_v.max);
+          double velocity=next_speed(timedA_.Get(),v,acc_v,dt);
+          SendMoveCmd(move_mode_,Monitor_emqx::eMPC, velocity, 0);
+          printf(" Vrtical :%f\n",velocity);
           continue;
         }
       }