fix multi-robots control bug

Author: Agnel-Wang

CMakeLists.txt

@@ -17,7 +17,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
 add_executable(z1_ctrl main.cpp)
 target_link_libraries(z1_ctrl libZ1_${CMAKE_HOST_SYSTEM_PROCESSOR}.so)
 
-find_package(gazebo)
-if(${gazebo_FOUND})
+find_package(catkin)
+if(${catkin_FOUND})
     add_subdirectory(sim)
 endif()

include/FSM/State_TeachRepeat.h

@@ -18,7 +18,6 @@ class State_TeachRepeat : public FSMState{
     bool _reachedStart = false;
     bool _finishedRepeat = false;
     size_t _index = 0;
-    size_t _indexPast;
     Vec6 _trajStartQ, _trajStartQd;
     double _trajStartGripperQ, _trajStartGripperQd;
 

include/control/armSDK.h

@@ -7,7 +7,7 @@
 class ARMSDK : public CmdPanel{
 public:
     ARMSDK(std::vector<KeyAction*> events, 
-        EmptyAction emptyAction, const char* IP, uint port, double dt = 0.002);
+        EmptyAction emptyAction, const char* sdkIP, uint sdkPort, uint ownPort, double dt = 0.002);
     ~ARMSDK();
     SendCmd getSendCmd();
     int getState(size_t channelID = 0);

include/message/LowlevelState.h

@@ -53,7 +53,7 @@ struct LowlevelState{
     double getGripperTauFiltered();
 private:
     size_t _dof = 6;
-    int temporatureLimit = 80.0;// centigrade
+    int temporatureLimit = 80;// centigrade
     std::vector<int> _isMotorConnectedCnt;
     std::vector<bool> _isMotorLostConnection;
 };

lib/libZ1_aarch64.so

@@ -45,7 +45,7 @@ int main(int argc, char **argv){
     ctrlComp->ioInter = new IOUDP(ctrlComp->ctrl_IP.c_str(), ctrlComp->ctrl_port);
     ctrlComp->geneObj();
     if(ctrlComp->ctrl == Control::SDK){
-        ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 0.002);
+        ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 8071, 0.002);
     }else if(ctrlComp->ctrl == Control::KEYBOARD){
         events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
         events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));

lib/libZ1_x86_64.so

@@ -48,7 +48,7 @@ int main(int argc, char **argv){
     ctrlComp->ioInter = new IOROS();
     ctrlComp->geneObj();
     if(ctrlComp->ctrl == Control::SDK){
-        ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 0.002);
+        ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 8071, 0.002);
     }else if(ctrlComp->ctrl == Control::KEYBOARD){
         events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
         events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));