1.添加提升机构控制模块

MPC/navigation.cpp

@@ -345,7 +345,7 @@ void Navigation::BrotherAgvStatuCallback(const MqttMsg &msg, void *context) {
     }
 }
 
-
+//未使用
 bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect) {
 
     while (cancel_ == false) {
@@ -658,14 +658,15 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
         }
 
         //下发速度
-        if (fabs(yawDiff) > 1 * M_PI / 180.0) {
-            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
-            actionType_ = eRotation;
-            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f diff:%f anyDirect:false\n",
-                   timedA_.Get(), out[0], yawDiff);
-        } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
+            Stop();
             return eMpcSuccess;
+        } else{
+            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
+            actionType_ = eRotation;
+            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
+                   timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
         }
         continue;
 
@@ -802,7 +803,8 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
         out.clear();
         out.push_back(0);
     } else {
-        double min_angular_velocity = 3.0 / 180 * M_PI;
+//        double min_angular_velocity = 1.0 / 180 * M_PI;
+        double min_angular_velocity = 0.00001;
         if (fabs(out[0]) < min_angular_velocity) {
             if (out[0] > 0)
                 out[0] = min_angular_velocity;

MPC/navigation_main.cpp

@@ -244,14 +244,15 @@ NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelb
         }
 
         //下发速度
-        if (fabs(yawDiff) > 1 * M_PI / 180.0) {
-            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
-            actionType_ = eRotation;
-            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f diff:%f anyDirect:false\n",
-                   timedA_.Get(), out[0], yawDiff);
-        } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
+        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
             printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
+            Stop();
             return eMpcSuccess;
+        } else{
+            SendMoveCmd(move_mode_, eRotation, 0, out[0]);
+            actionType_ = eRotation;
+            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
+                   timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
         }
         continue;
 

MPC/rotate_mpc.cpp

@@ -43,8 +43,8 @@ public:
         // 代价函数计算
         for (int t = 0; t < N - 1; ++t) {
             /// 角速度，角加速度weight loss
-            //fg[0] += angular_velocity_cost_weight * CppAD::pow(vars[N + t], 2);
-            //fg[0] += angular_acceleration_cost_weight * CppAD::pow(vars[N + t + 1] - vars[N + t], 2);
+            fg[0] += angular_velocity_cost_weight * CppAD::pow(vars[N + t], 2);
+            fg[0] += angular_acceleration_cost_weight * CppAD::pow(vars[N + t + 1] - vars[N + t], 2);
             /// 目标角度 loss
             fg[0] += angular_cost_weight * CppAD::pow(target_theta - vars[t], 2);
         }
@@ -120,9 +120,9 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     /// 定义最大最小角度,角速度,角加速度(不区分方向)
     double max_angular = M_PI;
-    double max_angular_velocity = 6.0 / 180 * M_PI;
-    double min_angular_velocity = 0.1 / 180 * M_PI;
-    double max_angular_acceleration_velocity = 3.0 / 180 * M_PI;//mpc_param.acc_angular; //
+    double max_angular_velocity = 15.0 / 180 * M_PI;
+    double min_angular_velocity = 0.01 / 180 * M_PI;
+    double max_angular_acceleration_velocity = 30.0 / 180 * M_PI;//mpc_param.acc_angular; //
     /// 调整输入
     if (angular_velocity > max_angular_velocity) angular_velocity = max_angular_velocity;
     if (angular_velocity < -max_angular_velocity) angular_velocity = -max_angular_velocity;
@@ -155,7 +155,8 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     // Lower and upper limits for the constraints
     /// 障碍物是否进入 碰撞检测范围内
-    float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
+//    float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
+    float distance = 1000;//屏蔽障碍物
     bool find_obs = distance < 5;
     if (find_obs) printf(" find obs ..............................\n");
     size_t n_constraints = 2 * N + find_obs * N;// 0~N角度，N~2N角加速度, 2N~3N障碍物约束
@@ -191,23 +192,25 @@ RotateMPC::solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter
 
     Pose2d targetAngularInAGV = current_to_target;
     /// 调整到适合的目标角度
-    if (targetAngularInAGV.theta() > 0) {
-        double acc_time = (max_angular_velocity - angular_velocity) / max_angular_acceleration_velocity;
-        double max_angular_once = max_angular_velocity * N * dt - acc_time * (max_angular_velocity - angular_velocity);
-        if (targetAngularInAGV.theta() > max_angular_once ) {
-            targetAngularInAGV.set_theta(max_angular_once);
-        }
-    }
-    else if (targetAngularInAGV.theta() < 0) {
-        double acc_time = fabs((-max_angular_velocity - angular_velocity) / -max_angular_acceleration_velocity);
-        double max_angular_once = -max_angular_velocity * N * dt + acc_time * (-max_angular_velocity - angular_velocity);
-        if (targetAngularInAGV.theta() < max_angular_once ) {
-            targetAngularInAGV.set_theta(max_angular_once);
-        }
-    } else{
-
-    }
-
+//    if (targetAngularInAGV.theta() > 0) {
+////        double acc_time = (max_angular_velocity - angular_velocity) / max_angular_acceleration_velocity;
+////        double max_angular_once = max_angular_velocity * N * dt - acc_time * (max_angular_velocity - angular_velocity);
+//        double max_angular_once = 9.0 / 180 * M_PI;
+//        if (targetAngularInAGV.theta() > max_angular_once ) {
+//            targetAngularInAGV.set_theta(max_angular_once);
+//        }
+//    }
+//    else if (targetAngularInAGV.theta() < 0) {
+////        double acc_time = fabs((-max_angular_velocity - angular_velocity) / -max_angular_acceleration_velocity);
+////        double max_angular_once = -max_angular_velocity * N * dt + acc_time * (-max_angular_velocity - angular_velocity);
+//        double max_angular_once = -9.0 / 180 * M_PI;
+//        if (targetAngularInAGV.theta() < max_angular_once ) {
+//            targetAngularInAGV.set_theta(max_angular_once);
+//        }
+//    } else{
+//
+//    }
+    printf("target: %f\n",targetAngularInAGV.theta()/M_PI*180);
 
     Eigen::VectorXd condition(4);
     condition << dt, targetAngularInAGV.theta(), obs_w_, obs_h_;

message.proto

@@ -15,7 +15,7 @@ message AgvStatu {
     float w = 2;
     int32 clamp = 3;    //夹持状态  0，中间状态，1夹紧到位，2，打开到位，
     int32 clamp_other = 4; //另外一节
-    int32 lifter = 5;   //提升机构
+    int32 lifter = 5;   //提升机构 0中间状态，1上升到位， 2下降到位
     int32 lifter_other = 6;     //另外一节
 }
 
@@ -91,7 +91,7 @@ service NavExcutor {
 
 message NavStatu
 {
-    int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关
+    int32 statu = 1; //0:ready  1:原地旋转，2：Horizon,3:vrtical,4:MPC,5:夹持开，6:夹持关，7:提升机构提升， 8: 提升机构下降
     bool main_agv = 2; //是否是两节控制plc
     int32 move_mode = 3; //运动模式，1：single，2：双车
     LidarOdomStatu odom = 4;