change joystick switch method

include/common/utilities/timer.h

@@ -7,21 +7,17 @@
 #include <sys/time.h>
 #include <sys/timerfd.h>
 
-//时间戳  微秒级， 需要#include <sys/time.h> 
 inline long long getSystemTime(){
     struct timeval t;  
     gettimeofday(&t, NULL);
     return 1000000 * t.tv_sec + t.tv_usec;  
 }
-//时间戳  秒级， 需要getSystemTime()
+
 inline double getTimeSecond(){
     double time = getSystemTime() * 0.000001;
     return time;
 }
-/*
-等待函数，微秒级，从startTime开始等待waitTime微秒
-目前不推荐使用，建议使用AbsoluteTimer类
-*/
+
 inline void absoluteWait(long long startTime, long long waitTime){
     if(getSystemTime() - startTime > waitTime){
         std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl

include/control/CtrlComponents.h

@@ -38,6 +38,7 @@ public:
     double collisionTLimit;
     bool isPlot;
     int trajChoose = 1;
+    size_t dogType = 1;//1:Aliengo 2:B1
 
     void geneObj();
     void writeData();

include/control/joystick.h

@@ -9,98 +9,22 @@
 #include <math.h>
 #include "message/udp.h"
 
-using namespace UNITREE_LEGGED_SDK_ALIENGO;
-// using namespace UNITREE_LEGGED_SDK_B1;
-
 class UnitreeJoystick : public CmdPanel{
 public:
-    UnitreeJoystick(std::vector<KeyAction*> events, 
+    UnitreeJoystick( size_t dogType, std::vector<KeyAction*> events, 
         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);
-        _udp->resetIO(BlockYN::NO, HIGH_STATE_LENGTH, 500000);
-        _udpCmd = {0};
-        _udpState = {0};
-        _readThread = new LoopFunc("JoyStickRead", 0.0, boost::bind(&UnitreeJoystick::_read, this));
-        _runThread = new LoopFunc("CmdPanelRun", _dt, boost::bind(&UnitreeJoystick::_run, this));
-        _readThread->start();
-        _runThread->start();
-    };
-    ~UnitreeJoystick(){
-        delete _runThread;
-        delete _readThread;
-        delete _udp;
-    };
+        double dt = 0.002);
+    ~UnitreeJoystick();
 private:
-    void _read(){
-        _udp->send((uint8_t*)&_udpCmd, HIGH_CMD_LENGTH);
-        _udp->recv((uint8_t*)&_udpState);
-
-        #ifdef CTRL_BY_ALIENGO_JOYSTICK
-        memcpy(&_keyData, _udpState.wirelessRemote, 40);
-        #else
-        memcpy(&_keyData, &_udpState.wirelessRemote[0], 40);
-        #endif
-
-        _extractCmd();
-        _updateState();
-    };
-    void _extractCmd(){
-        float joyThre = 0.5;    // 手柄数值范围 +-1
-
-        if(((int)_keyData.btn.components.L1 == 1) &&
-        ((int)_keyData.btn.components.L2 == 1)){
-            _keyCmd.c = "l12";
-        }else if((int)_keyData.btn.components.R2  == 1){
-            _keyCmd.c = "r2";
-            if((int)_keyData.btn.components.X  == 1){
-                _keyCmd.c = "r2x";
-            }
-        }else if((int)_keyData.btn.components.R1  == 1){
-            _keyCmd.c = "r1";
-        }else if(fabs(_keyData.lx) > joyThre){
-            _keyData.lx > joyThre?_keyCmd.c = "left_left":_keyCmd.c = "left_right";
-        }else if(fabs(_keyData.ly) > joyThre){
-            _keyData.ly > joyThre?_keyCmd.c = "left_up":_keyCmd.c = "left_down";
-        }else if((int)_keyData.btn.components.up  == 1){
-            _keyCmd.c = "up";
-        }else if((int)_keyData.btn.components.down  == 1){
-            _keyCmd.c = "down";
-        }else if(fabs(_keyData.rx) > joyThre){
-            _keyData.rx > joyThre?_keyCmd.c = "right_left":_keyCmd.c = "right_right";
-        }else if(fabs(_keyData.ry) > joyThre){
-            _keyData.ry > joyThre?_keyCmd.c = "right_up":_keyCmd.c = "right_down";
-        }else if((int)_keyData.btn.components.Y  == 1){
-            _keyCmd.c = "Y";
-        }else if((int)_keyData.btn.components.A  == 1){
-            _keyCmd.c = "A";
-        }else if((int)_keyData.btn.components.X  == 1){
-            _keyCmd.c = "X";
-        }else if((int)_keyData.btn.components.B  == 1){
-            _keyCmd.c = "B";
-        }else if((int)_keyData.btn.components.right  == 1){
-            _keyCmd.c = "left";
-        }else if((int)_keyData.btn.components.left  == 1){
-            _keyCmd.c = "right";
-        }else if((int)_keyData.btn.components.up  == 1){
-            _keyCmd.c = "up";
-        }else if((int)_keyData.btn.components.down  == 1){
-            _keyCmd.c = "down";
-        }else if((int)_keyData.btn.components.select  == 1){
-            _keyCmd.c = "select";
-        }else{
-            _releaseKeyboard();
-            return;
-        }
-
-        _pressKeyboard();
-    };
-
+    void _read();
+    void _extractCmd();
+    size_t _dogType;
     xRockerBtnDataStruct _keyData;
     UDPPort *_udp;
-    HighCmd _udpCmd;
-    HighState _udpState;
+    UNITREE_LEGGED_SDK_ALIENGO::HighCmd _udpCmdAliengo;
+    UNITREE_LEGGED_SDK_ALIENGO::HighState _udpStateAliengo;
+    UNITREE_LEGGED_SDK_B1::HighCmd _udpCmdB1;
+    UNITREE_LEGGED_SDK_B1::HighState _udpStateB1;
 };
 
 #endif  // _UNITREE_ARM_JOYSTICK_H_

include/interface/IOUDP.h

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

main.cpp

@@ -50,7 +50,6 @@ int main(int argc, char **argv){
     ros::init(argc, argv, "z1_controller");
 #endif
 
-    // ctrlComp->isPlot = true;
     ctrlComp->dt = 1.0/250.;
     ctrlComp->armConfigPath =  "../config/";
     ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv");
@@ -96,7 +95,7 @@ int main(int argc, char **argv){
         events.push_back(new ValueAction("Y",           "A",            0.5));//Rot_Z
         events.push_back(new ValueAction("right",       "left",         1.0));//girpper, close-open
 
-        ctrlComp->cmdPanel = new UnitreeJoystick(events, emptyAction);
+        ctrlComp->cmdPanel = new UnitreeJoystick(ctrlComp->dogType, events, emptyAction);
     }
     std::vector<FSMState*> states;
     states.push_back(new State_Passive(ctrlComp));