backup

include/armcontroller.hpp

@@ -118,7 +118,7 @@ private:
     msg_data msgData_{};
     // anglePos anglePos_{};
     uint seq = 0;                                                           // 唯一识别码
-    uint funcdoe = 0;                                                       // 指令功能码
+    uint funcdoe = 3;                                                       // 指令功能码
     uint address = 0;                                                       // 指令地址码
     uint mode = 0;                                                          // 控制模式
     unitree_arm::msg::dds_::ArmString_ msg_{};                              // 传送消息

src/armController.cpp

@@ -186,6 +186,7 @@ void armController::angleLRAction(double lr)
         this->msgData_.angle0 += lr;
     else
         this->msgData_.angle0 = this->msgData_.angle0 > 0 ? 135 : -135;
+    this->mode = 0;
     this->setAngle(this->msgData_);
 }
 
@@ -207,6 +208,7 @@ void armController::angleUpDownAction(double j1, double j2, double j4)
     else
         this->msgData_.angle4 = this->msgData_.angle4 > 0 ? 90 : -90;
 
+    this->mode = 0;
     this->setAngle(this->msgData_);
 }
 
@@ -216,24 +218,53 @@ void armController::angleRotateAction(double j5)
         this->msgData_.angle5 += j5;
     else
         this->msgData_.angle5 = this->msgData_.angle5 > 0 ? 135 : -135;
+    this->mode = 0;
     this->setAngle(this->msgData_);
 }
 
 // 扔垃圾
 void armController::throwTrash()
 {
-    this->msgData_.angle0 = 70.9;
+    // this->msgData_.angle0 = 51.5;
-    this->msgData_.angle1 = 20.0;
+    // this->msgData_.angle1 = 39.400;
-    this->msgData_.angle2 = 38.7;
+    // this->msgData_.angle2 = -15.3;
-    this->msgData_.angle3 = 10.1;
+    // this->msgData_.angle3 = 2.2;
-    this->msgData_.angle4 = 8.7;
+    // this->msgData_.angle4 = 66.09999;
-    this->msgData_.angle5 = -1.4;
+    // this->msgData_.angle5 = 48.5;
+
+    this->mode = 1;
+
+    this->msgData_.angle0 = -0.899;
+    this->msgData_.angle1 = 37.2;
+    this->msgData_.angle2 = -39.2;
+    this->msgData_.angle3 = 1.7999;
+    this->msgData_.angle4 = 45.299;
+    this->msgData_.angle5 = -2.799;
 
     this->setAngle(this->msgData_);
 
-    std::this_thread::sleep_for(std::chrono::seconds(3));
+    // this->msgData_.angle0 = 41.700;
+    // this->msgData_.angle1 = 1.7999;
+    // this->msgData_.angle2 = 26.3999;
+    // this->msgData_.angle3 = 4.19999;
+    // this->msgData_.angle4 = 11.3000;
+    // this->msgData_.angle5 = -0.6000;
+
+    // this->setAngle(this->msgData_);
+
+    this->msgData_.angle0 = 54.40;
+    this->msgData_.angle1 = 20.7000;
+    this->msgData_.angle2 = 26.5;
+    this->msgData_.angle3 = 5.300;
+    this->msgData_.angle4 = 10.800;
+    this->msgData_.angle5 = 40.0;
+
+    this->setAngle(this->msgData_);
+
+    this->msgData_.angle6 = 44.7;
+
+    this_thread::sleep_for(chrono::seconds(5));
 
-    this->msgData_.angle6 = 68;
     this->setAngle(this->msgData_);
 }
 
@@ -259,6 +290,7 @@ void armController::jawRelease()
         this->msgData_.angle6 += 30;
     else
         this->msgData_.angle6 = this->msgData_.angle6 > 0 ? 105 : -105;
+    this->mode = 0;
     this->setAngle(this->msgData_);
 }
 
@@ -269,6 +301,7 @@ void armController::jawClaming()
         this->msgData_.angle6 -= 30;
     else
         this->msgData_.angle6 = this->msgData_.angle6 > 0 ? 105 : -105;
+    this->mode = 0;
     this->setAngle(this->msgData_);
 }
 
@@ -295,6 +328,21 @@ void armController::goHome()
 // 捡垃圾预设动作
 void armController::trashActionDefault()
 {
+    // this->mode = 0;
+
+    // this->msgData_.angle0 = -0.899;
+    // this->msgData_.angle1 = 37.2;
+    // this->msgData_.angle2 = -39.2;
+    // this->msgData_.angle3 = 1.7999;
+    // this->msgData_.angle4 = 45.299;
+    // this->msgData_.angle5 = -2.799;
+
+    // this->setAngle(this->msgData_);
+
+    // this_thread::sleep_for(chrono::seconds(1));
+
+    this->mode = 0;
+
     this->msgData_.angle0 = -0.8;
     this->msgData_.angle1 = 82.5;
     this->msgData_.angle2 = -45.7;
@@ -304,7 +352,7 @@ void armController::trashActionDefault()
     this->msgData_.angle6 = 66.0999984741211;
 
     this->address = 1;
-    this->mode = 0;
+
     this->funcdoe = 2;
 
     this->setAngle(this->msgData_);

src/main.cpp

@@ -139,8 +139,18 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, armController *ar
     double x, y, z, roll, pitch, yaw;
     double angleData = 2.0;
 
+    // 冷却时间
+    auto lastExec = chrono::steady_clock::now() - chrono::seconds(10);
+    auto nowExec = chrono::steady_clock::now();
+    const auto cooldown = std::chrono::seconds(8);
+
+    auto ZlastExec = chrono::steady_clock::now() - chrono::seconds(10);
+    auto ZnowExec = chrono::steady_clock::now();
+    const auto Zcooldown = std::chrono::seconds(1);
+
     while (true)
     {
+        ZnowExec = chrono::steady_clock::now();
         // 接收客户端发送的数据
         recvData = Socket->ReceiveData();
         if (recvData.empty())
@@ -166,6 +176,9 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, armController *ar
         if (controlStatus == false)
         { // 机械臂
             cout << "ARM:" << recvData << endl;
+            if (ZnowExec - ZlastExec < Zcooldown)
+                continue;
+            ZlastExec = ZnowExec;
             switch (static_cast<char>(recvData[0]))
             {
             case 'W':
@@ -199,8 +212,16 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, armController *ar
                 arm->angleRotateAction(angleData);
                 break;
             case 'P':
+            {
+                nowExec = chrono::steady_clock::now();
+                if (nowExec - lastExec > cooldown)
+                {
                     arm->throwTrash();
+                    lastExec = nowExec;
+                }
                 break;
+            }
+
             // case 'B':
             //     yaw += 0.05;
             //     break;
@@ -224,6 +245,13 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, armController *ar
         { // 机械狗
           //  根据接收到的指令控制机器人
             cout << "DOG:" << recvData << endl;
+            if (recvData.find("L") != string::npos)
+            {
+                sport_client.StopMove();
+                isMoving = false;
+                continue;
+            }
+
             switch (static_cast<char>(recvData[0]))
             {
             case 'P': // 按 'p' 键退出
@@ -303,6 +331,7 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, armController *ar
                 break;
             }
         }
+        this_thread::sleep_for(chrono::milliseconds(500));
     }
 }