Browse Source

1.添加提升控制

gf 1 year ago
parent
commit
b886ff03e3
4 changed files with 61 additions and 42 deletions
  1. 10 0
      ControllWidget.py
  2. 12 0
      RobotData.py
  3. 35 38
      map.json
  4. 4 4
      message.proto

+ 10 - 0
ControllWidget.py

@@ -120,6 +120,10 @@ class ControlFrame(QFrame):
         self.btnClampCheck.setGeometry(260, 160, 60, 40)
         self.btnClampCheck.clicked.connect(self.ClampClick)
 
+        self.btnLifterCheck = QCheckBox("提升", self)
+        self.btnLifterCheck.setGeometry(360, 160, 60, 40)
+        self.btnLifterCheck.clicked.connect(self.LifterClick)
+
         self.btnAuto = QCheckBox(self)
         self.btnAuto.setText("自动测试")
         self.btnAuto.setGeometry(260, 195, 100, 40)
@@ -209,6 +213,12 @@ class ControlFrame(QFrame):
         else:
             self.threadPool_.submit(self.robot_.ClampOpen)
 
+    def LifterClick(self):
+        if self.robot_.IsLiferDowned() == False:
+            self.threadPool_.submit(self.robot_.LiferDown)
+        else:
+            self.threadPool_.submit(self.robot_.LiferRise)
+
     def MainAgvchangecb(self):
         if self.robot_.IsMainModeStatu() == False:
             leng = float(self.WheelBaseEdit.text())

+ 12 - 0
RobotData.py

@@ -459,6 +459,18 @@ class Robot(MqttAsync):
 
         return False
 
+    def IsLiferDowned(self):
+        if self.timedRobotStatu_.timeout() == False:
+            if self.IsMainModeStatu():
+                clamp = self.timedRobotStatu_.statu.agvStatu.clamp
+                clamp_other = self.timedRobotStatu_.statu.agvStatu.clamp_other
+                return (clamp == 1 and clamp_other == 1)
+            else:
+                return self.timedRobotStatu_.statu.agvStatu.clamp == 1
+
+        return False
+
+
     def IsClampRunning(self):
         if self.timedRobotStatu_.timeout() == False:
             if self.IsMainModeStatu():

+ 35 - 38
map.json

@@ -1,42 +1,39 @@
-
 {
-  "street_nodes":{
-    "V1":[2.94,-0.42],
-    "S1":[2.94,3.58],
-    "V2": [14.07,-0.42],
-    "V3": [10.25,-0.42],
-    "V4": [5.37,-0.42],
-    "V5": [20.43,-0.42],
-    "V6": [22.78,-0.42],
-    "V7": [5.37,16.27],
-    "S4":[5.37,3.58],
-    "N1":[26.65,-0.42],
-    "H1": [26.65,11.5],
-    "H2": [26.65,16.27],
-    "N2":[33,-0.42]
+  "street_nodes": {
+    "R1": [
+      -2.4,
+      -8.1
+    ],
+    "R2": [
+      -2.4,
+      -14.88
+    ],
+    "R3": [
+      -10.62,
+      -14.88
+    ]
   },
-  
-  "space_nodes":{
-    "PW":[5.37,-8.8,-1.57],
-    "I":[33,16.27,0],
-    "P":[2.94,-6.05,-1.57],
-    "PA":[10.25,5.17,1.57],
-    "PB":[22.78,5.17,1.57],
-    "PX":[20.43,-6.14,0],
-    "PY":[22.78,-6.14,-1.57]
+
+  "space_nodes": {
+    "S1": [
+      -10.62,
+      -4.35,
+      1.57
+    ]
   },
-  "roads":{
-    "road1":["I","H2"],
-    "road2":["H1","N1","H2"],
-    "road3":["V7","H2"],
-    "conn":["V1","V2","V3","V4","V5","V6","N1","N2"],
-    "rp":["V4","PW"],
-    "vp1":["V1","P"],
-    "vp2":["V3","PA"],
-    "s_conn_1":["V1","S1"],
-    "s_conn_4":["V4","S4"],
-    "s_conn_5":["V5","PX"],
-    "CPY":["V6","PY"],
-    "CPB":["V6","PB"]
+
+  "roads": {
+    "road1": [
+      "R1",
+      "R2"
+    ],
+    "road2": [
+      "R2",
+      "R3"
+    ],
+    "road3": [
+      "R3",
+      "S1"
+    ]
   }
-}
+}

+ 4 - 4
message.proto

@@ -13,15 +13,15 @@ message AgvStatu{
   float w=2;
   int32 clamp=3;    //夹持状态  0,中间状态,1夹紧到位,2,打开到位,
   int32 clamp_other=4; //另外一节
-  int32 lifter=5;
-  int32 lifter_other=6;
+  int32 lifter=5;   //提升机构
+  int32 lifter_other = 6;     //另外一节
 
 }
 
 message ToAgvCmd {
   int32 H=1;  //心跳
   int32 M=2;	//模式:2整车模式,1:单车
-  int32 T=3; // 1:原地旋转,2:横移,3:MPC巡线/前进, 5,夹持,6,松夹持,其他/未接收到:停止
+  int32 T=3;  // 1:原地旋转, 2:横移, 3:MPC巡线/前进, 5:夹持, 6:松夹持, 7:提升机构抬升, 8:抬升机构下降, 其他/未接收到:停止
   float V=4;  //角速度
   float W=5;  //线速度
   float L=6;  //轴距
@@ -53,7 +53,7 @@ message Trajectory
 //----------------------------
 message NewAction //进库,出库,轨迹导航,夹持,松夹持
 {
-  int32 type =1; //1,进库,2,出库,3,自动选择动作导航,4,保证agv朝前导航,5,汽车模型导航,6:夹持,7:松夹持,8:切换模式
+  int32 type =1; //1,进库,2,出库,3,自动选择动作导航,4,保证agv朝前导航,5,汽车模型导航,6:夹持,7:松夹持,8:切换模式, 9:提升机构提升, 10: 提升机构下降
   PathNode spaceNode = 2; //进出库起点
   PathNode passNode=3; //进出库途径点
   PathNode streetNode = 4; //进出库终点