diff --git a/include/common/utilities/timer.h b/include/common/utilities/timer.h index 40701da..ad1ad59 100644 --- a/include/common/utilities/timer.h +++ b/include/common/utilities/timer.h @@ -7,21 +7,17 @@ #include #include -//时间戳 微秒级， 需要#include 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 diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h index ed0524e..bc508ae 100755 --- a/include/control/CtrlComponents.h +++ b/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(); diff --git a/include/control/joystick.h b/include/control/joystick.h index c20cd94..f85d9c7 100644 --- a/include/control/joystick.h +++ b/include/control/joystick.h @@ -9,98 +9,22 @@ #include #include "message/udp.h" -using namespace UNITREE_LEGGED_SDK_ALIENGO; -// using namespace UNITREE_LEGGED_SDK_B1; - class UnitreeJoystick : public CmdPanel{ public: - UnitreeJoystick(std::vector events, + UnitreeJoystick( size_t dogType, std::vector 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_ diff --git a/include/interface/IOUDP.h b/include/interface/IOUDP.h index f9c267c..0b4fa05 100644 --- a/include/interface/IOUDP.h +++ b/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); diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so index 7bbaf32..acbac09 100644 Binary files a/lib/libZ1_ROS_Linux64.so and b/lib/libZ1_ROS_Linux64.so differ diff --git a/lib/libZ1_UDP_Linux64.so b/lib/libZ1_UDP_Linux64.so index b731d14..a2a2d16 100644 Binary files a/lib/libZ1_UDP_Linux64.so and b/lib/libZ1_UDP_Linux64.so differ diff --git a/main.cpp b/main.cpp index 60b0c9f..22fc687 100755 --- a/main.cpp +++ b/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 states; states.push_back(new State_Passive(ctrlComp));