150 lines
3.7 KiB
C++
150 lines
3.7 KiB
C++
/*****************************************************************
|
||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||
*****************************************************************/
|
||
#ifndef _UNITREE_ARM_CMDPANEL_H_
|
||
#define _UNITREE_ARM_CMDPANEL_H_
|
||
|
||
#include <vector>
|
||
#include <deque>
|
||
#include "unitree_arm_sdk/loop.h"
|
||
#include "unitree_arm_sdk/common/arm_common.h"
|
||
|
||
|
||
enum class KeyPress{
|
||
RELEASE,
|
||
PRESS,
|
||
REPEAT
|
||
};
|
||
|
||
|
||
enum class ActionType{
|
||
EMPTY,
|
||
STATE,
|
||
VALUE
|
||
};
|
||
|
||
|
||
struct KeyCmd{
|
||
std::string c;
|
||
KeyPress keyPress;
|
||
};
|
||
|
||
|
||
class KeyAction{
|
||
public:
|
||
KeyAction(ActionType type);
|
||
virtual ~KeyAction(){};
|
||
ActionType getType(){return _type;}
|
||
protected:
|
||
ActionType _type;
|
||
};
|
||
|
||
|
||
class StateAction : public KeyAction{
|
||
public:
|
||
StateAction(std::string c, int state, KeyPress press = KeyPress::PRESS);
|
||
virtual ~StateAction(){};
|
||
int getState(){return _state;};
|
||
bool handleCmd(KeyCmd keyCmd, int &state);
|
||
protected:
|
||
int _state;
|
||
KeyCmd _keyCmdSet;
|
||
};
|
||
|
||
class EmptyAction : public StateAction{
|
||
public:
|
||
EmptyAction(int state);
|
||
~EmptyAction(){};
|
||
private:
|
||
|
||
};
|
||
|
||
|
||
/*
|
||
必须为长按, deltaValue为每秒改变量
|
||
valueAction允许共用按键，例如空格停止
|
||
但是正反转、停止键不可重复
|
||
*/
|
||
class ValueAction : public KeyAction{
|
||
public:
|
||
ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0);
|
||
ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double initValue = 0.0);
|
||
ValueAction(std::string cUp, std::string cDown, double deltaValue, double limit1, double limit2, double initValue = 0.0);
|
||
ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double limit1, double limit2, double initValue = 0.0);
|
||
|
||
~ValueAction(){};
|
||
bool handleCmd(KeyCmd keyCmd);
|
||
void setDt(double dt);
|
||
double getValue();
|
||
double getDValue();
|
||
double setValue(double value){_value = value;}
|
||
private:
|
||
double _value;
|
||
double _changeDirection;
|
||
double _dV = 0.0;
|
||
double _dt = 0.0;
|
||
double _dVdt = 0.0;
|
||
double _lim1, _lim2;
|
||
bool _hasLim = false;
|
||
bool _hasGoZero = false;
|
||
|
||
KeyCmd _upCmdSet;
|
||
KeyCmd _downCmdSet;
|
||
KeyCmd _goZeroCmdSet;
|
||
|
||
};
|
||
|
||
|
||
class CmdPanel{
|
||
public:
|
||
CmdPanel(std::vector<KeyAction*> events,
|
||
EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
|
||
virtual ~CmdPanel();
|
||
int getState(size_t channelID = 0);
|
||
std::vector<double> getValues();
|
||
std::vector<double> getDValues();
|
||
void setValue(std::vector<double> values);
|
||
void setValue(double value, size_t id);
|
||
virtual std::string getString(std::string slogan);
|
||
virtual std::vector<double> stringToArray(std::string slogan);
|
||
virtual std::vector<std::vector<double> > stringToMatrix(std::string slogan);
|
||
|
||
virtual SendCmd getSendCmd();
|
||
virtual void getRecvState(RecvState recvState);
|
||
|
||
protected:
|
||
virtual void _read() = 0;
|
||
void _start();
|
||
void _run();
|
||
void _updateState();
|
||
void _pressKeyboard();
|
||
void _releaseKeyboard();
|
||
|
||
static void* _runStatic(void* obj);
|
||
static void* _readStatic(void* obj);
|
||
|
||
LoopFunc *_runThread;
|
||
LoopFunc *_readThread;
|
||
|
||
std::vector<StateAction> _stateEvents;
|
||
std::vector<ValueAction> _valueEvents;
|
||
|
||
EmptyAction _emptyAction;
|
||
size_t _actionNum = 0;
|
||
size_t _stateNum = 0;
|
||
size_t _valueNum = 0;
|
||
size_t _channelNum;
|
||
std::vector<double> _values;
|
||
std::vector<double> _dValues;
|
||
int _state;
|
||
std::vector<std::deque<int>> _stateQueue;
|
||
std::vector<int> _outputState;
|
||
std::vector<bool> _getState;
|
||
double _dt;
|
||
KeyCmd _keyCmd;
|
||
std::string _cPast = "";
|
||
|
||
bool _running = true;
|
||
};
|
||
|
||
#endif // _UNITREE_ARM_CMDPANEL_H_
|