init

include/armcontroller.hpp

@@ -14,6 +14,7 @@
 #include <kdl/frames.hpp>                     //位姿工具提供
 #include <kdl/chainfksolverpos_recursive.hpp> //前向运动学
 #include <kdl/chainiksolverpos_lma.hpp>       //逆向运动学
+#include <thread>
 
 #define TOPIC "rt/arm_Command"
 #define TOPICSTATUS "current_servo_angle"
@@ -60,10 +61,10 @@ public:
 
     // 从urdf构建base->tip的kdl::chain,并返回关节顺序(后续角度填写顺序)
     bool buildChainFromUrdf(const string &urdfPath, const string &baseLink, const string &tipLink, KDL::Chain &outChain, vector<string> &outJointNames);
-    bool ikPoseRPY(double x,double y,double z,double roll,double pitch,double yaw);
+    bool ikPoseRPY(double x, double y, double z, double roll, double pitch, double yaw);
 
     // 根据角度求坐标
-    bool getCurrentPoseFromMsg(double &x, double &y, double &z, double &roll, double &pitch, double &yaw);
+    bool getCurrentPoseFromMsg(double &x,double &y,double &z,double &roll,double &pitch,double &yaw);
 
     // 填充至JntArray
     void fillToJntArray(const msg_data &msgdata_, KDL::JntArray &qSol);
@@ -72,14 +73,20 @@ public:
     void fillToMsgData(const KDL::JntArray &qSol, msg_data &msgdata);
 
     ///////基本控制///////
-    // 夹爪前进
-    void goForward();
+
+    void goAction(const double x,const double y,const double z,const double roll,const double pitch,const double yaw);
+
+    // 电机动作
+    void angleLRAction(double lr);
 
     // 夹爪后退
-    void goBack();
+    void angleUpDownAction(double j1,double j2,double j4);
 
     // 夹爪向上
-    void goUp();
+    void angleRotateAction(double j5);  
+
+    //扔垃圾
+    void throwTrash();
 
     // 夹爪向下
     void goDown();
@@ -109,6 +116,7 @@ public:
 private:
     // 发送消息
     msg_data msgData_{};
+    // anglePos anglePos_{};
     uint seq = 0;                                                           // 唯一识别码
     uint funcdoe = 0;                                                       // 指令功能码
     uint address = 0;                                                       // 指令地址码