修复原地调整斜坡bug

MPC/navigation.cpp

@@ -365,29 +365,29 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
       double dt=0.1;
       if (action == 1) {
         if (Pose2d::abs(diff).theta() > thresh.theta()) {
-          double theta = (diff.theta(),limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
+          double theta = limit(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 angular:%f,target:%f\n",angular,theta);
+          std::cout<<"current:"<<timedPose_.Get()<<"  target:"<<target<<" diff:"<<diff<<std::endl;
+          printf(" Ratate | input anguar:%f,next angular:%f,target:%f\n",timedA_.Get(),angular,theta);
           continue;
         }
       }
       if (action == 2) {
         if (Pose2d::abs(diff).y() > thresh.y()) {
           double hv=limit(diff.y(),limit_h.min,limit_h.max);
-          double velocity=next_speed(timedA_.Get(),hv,acc_v,dt);
+          double velocity=next_speed(timedV_.Get(),hv,acc_v,dt);
           SendMoveCmd(move_mode_,Monitor_emqx::eHorizontal,velocity , 0);
-          printf(" Horizontal :%f\n",velocity);
+          printf(" Horizontal input:%f out:%f\n",timedV_.Get(),velocity);
           continue;
         }
       }
       if (action == 3) {
         if (Pose2d::abs(diff).x() > thresh.x()) {
           double v=limit(diff.x(),limit_v.min,limit_v.max);
-          double velocity=next_speed(timedA_.Get(),v,acc_v,dt);
+          double velocity=next_speed(timedV_.Get(),v,acc_v,dt);
           SendMoveCmd(move_mode_,Monitor_emqx::eMPC, velocity, 0);
-          printf(" Vrtical :%f\n",velocity);
+          printf(" Vrtical input:%f,out:%f\n",timedV_.Get(),velocity);
           continue;
         }
       }