add version info

config/config.xml

@@ -1,11 +1,5 @@
 <configurartion>
-    <control set=2>
+    <!-- lower computer communication -->
-        <option1>keyboard</option1>
-        <option2>sdk</option2>
-        <option3>joystick</option3> <!-- default aliengo_joystick -->
-    </control>
-
-    <!-- slave computer communication -->
     <IP>192.168.123.110</IP>
     <Port>8881</Port>
 
@@ -14,4 +8,5 @@
         <limitT>10.0</limitT> <!-- N*M -->
         <load>0.0</load><!-- kg-->
     </collision>
+    <!-- <plot set=1/> -->
 </configurartion>

include/control/CtrlComponents.h

@@ -17,7 +17,7 @@ using namespace std;
 
 struct CtrlComponents{
 public:
-    CtrlComponents();
+    CtrlComponents(int argc, char**argv);
     ~CtrlComponents();
 
     std::string armConfigPath;
@@ -42,6 +42,8 @@ public:
     void geneObj();
     void writeData();
 private:
+    void inputProcess(int argc, char** argv);
+
     std::string ctrl_IP;
     uint ctrl_port;
     double _loadWeight;

include/control/joystick.h

@@ -2,12 +2,12 @@
 #define _UNITREE_ARM_JOYSTICK_H_
 
 #include <vector>
-#include "message/udp.h"
 #include "control/cmdPanel.h"
 #include "message/joystick_common.h"
 #include "message/aliengo_common.h"
 #include "message/b1_common.h"
 #include <math.h>
+#include "message/udp.h"
 
 using namespace UNITREE_LEGGED_SDK_ALIENGO;
 // using namespace UNITREE_LEGGED_SDK_B1;
@@ -18,8 +18,8 @@ public:
         EmptyAction emptyAction, size_t channelNum = 1,
         double dt = 0.002)
         :  CmdPanel(events, emptyAction, channelNum, dt){
-        _udp = new UDPPort("dog", "192.168.123.220", 8082, 8081, HIGH_STATE_LENGTH, BlockYN::NO, 500000);
+        _udp = new UDPPort("dog", "192.168.123.220", 8082, 8081);
-
+        _udp->resetIO(BlockYN::NO, HIGH_STATE_LENGTH, 500000);
         _udpCmd = {0};
         _udpState = {0};
         _readThread = new LoopFunc("JoyStickRead", 0.0, boost::bind(&UnitreeJoystick::_read, this));
@@ -28,10 +28,9 @@ public:
         _runThread->start();
     };
     ~UnitreeJoystick(){
-        delete _udp;
         delete _runThread;
         delete _readThread;
-        
+        delete _udp;
     };
 private:
     void _read(){

include/interface/IOUDP.h

@@ -5,7 +5,7 @@
 
 class IOUDP : public IOInterface{
 public:
-    IOUDP(const char* IP, uint port);
+    IOUDP(const char* IP, uint port, bool showInfo = true);
     ~IOUDP();
 
     bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);

include/message/udp.h

@@ -17,18 +17,19 @@ enum class BlockYN{
 
 class IOPort{
 public:
-    IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs)
+    IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs, bool showInfo)
         :_name(name){
-        resetIO(blockYN, recvLength, timeOutUs);
+        resetIO(blockYN, recvLength, timeOutUs, showInfo);
     }
     virtual ~IOPort(){}
     virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
     virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
     virtual size_t recv(uint8_t *recvMsg) = 0;
     virtual bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec) = 0;
-    void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
+    void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs, bool showInfo = true);
     bool isDisConnect = false;
 protected:
+    bool _showInfo;
     std::string _name;
     BlockYN _blockYN = BlockYN::NO;
     size_t _recvLength;
@@ -43,7 +44,8 @@ public:
     UDPPort(std::string name, std::string toIP, uint toPort, uint ownPort, 
             size_t recvLength = 0,
             BlockYN blockYN = BlockYN::NO,
-            size_t timeOutUs = 20000);
+            size_t timeOutUs = 20000,
+            bool showInfo = true);
     ~UDPPort();
     size_t send(uint8_t *sendMsg, size_t sendMsgLength);
     size_t recv(uint8_t *recvMsg, size_t recvLength);

main.cpp

@@ -17,7 +17,6 @@
 #include "FSM/State_LowCmd.h"
 #include "FSM/State_SetTraj.h"
 
-const std::string Z1_CTRL_VERSION = "2022.11.11";
 bool running = true;
 
 // over watch the ctrl+c command
@@ -32,18 +31,11 @@ void setProcessScheduler(){
     sched_param param;
     param.sched_priority = sched_get_priority_max(SCHED_FIFO);
     if (sched_setscheduler(pid, SCHED_FIFO, &param) == -1) {
-        std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl;
+        // std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl;
     }
 }
 
 int main(int argc, char **argv){
-    if(argc == 2) {
-        if((strcmp(argv[1], "-v") == 0) || (strcmp(argv[1], "--version") == 0)){
-            std::cout << "Version z1_controller: "<< Z1_CTRL_VERSION<<std::endl;
-            return 0;
-        }
-    }
-    
     /* set real-time process */
     setProcessScheduler();
     /* set the print format */
@@ -52,18 +44,7 @@ int main(int argc, char **argv){
 
     EmptyAction emptyAction((int)ArmFSMStateName::INVALID);
     std::vector<KeyAction*> events;
-    CtrlComponents *ctrlComp = new CtrlComponents();
+    CtrlComponents *ctrlComp = new CtrlComponents(argc, argv);
-
-    // control method
-    if(argc == 2) {
-        if(argv[1][0] == 'k'){
-            ctrlComp->ctrl = Control::KEYBOARD;
-        }else if(argv[1][0] == 's'){
-            ctrlComp->ctrl = Control::SDK;
-        }else if(argv[1][0] == 'j'){
-            ctrlComp->ctrl = Control::JOYSTICK;
-        }
-    }
     
 #ifdef COMPILE_WITH_SIMULATION
     ros::init(argc, argv, "z1_controller");
@@ -143,7 +124,6 @@ int main(int argc, char **argv){
         usleep(100000);
     }
 
-    delete fsm;
     delete ctrlComp;
     return 0;
 }