2022-07-20 11:11:38 +08:00
|
|
|
#ifndef FSMSTATE_H
|
|
|
|
|
#define FSMSTATE_H
|
|
|
|
|
|
|
|
|
|
#include <string>
|
|
|
|
|
#include <iostream>
|
|
|
|
|
#include <unistd.h>
|
|
|
|
|
#include "control/CtrlComponents.h"
|
|
|
|
|
#include "common/math/mathTools.h"
|
2022-11-11 19:49:41 +08:00
|
|
|
#include "common/utilities/timer.h"
|
|
|
|
|
#include "FSM/BaseState.h"
|
2022-07-20 11:11:38 +08:00
|
|
|
|
|
|
|
|
class FSMState : public BaseState{
|
|
|
|
|
public:
|
|
|
|
|
FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString);
|
|
|
|
|
virtual ~FSMState(){}
|
|
|
|
|
|
|
|
|
|
virtual void enter() = 0;
|
|
|
|
|
virtual void run() = 0;
|
|
|
|
|
virtual void exit() = 0;
|
|
|
|
|
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
|
2022-11-11 19:49:41 +08:00
|
|
|
bool _collisionTest();
|
2022-09-13 19:53:15 +08:00
|
|
|
|
2022-07-20 11:11:38 +08:00
|
|
|
protected:
|
|
|
|
|
void _armCtrl();
|
2022-11-11 19:49:41 +08:00
|
|
|
void _recordData();
|
|
|
|
|
Vec6 _postureToVec6(Posture posture);
|
2022-07-20 11:11:38 +08:00
|
|
|
|
|
|
|
|
LowlevelCmd *_lowCmd;
|
|
|
|
|
LowlevelState *_lowState;
|
2022-11-11 19:49:41 +08:00
|
|
|
IOInterface *_ioInter;
|
|
|
|
|
ArmModel *_armModel;
|
2022-07-20 11:11:38 +08:00
|
|
|
|
2022-11-11 19:49:41 +08:00
|
|
|
Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauf, _tauCmd, _g;
|
|
|
|
|
double _gripperPos, _gripperW, _gripperTau;
|
|
|
|
|
uint _collisionCnt;
|
2022-07-20 11:11:38 +08:00
|
|
|
|
2022-11-11 19:49:41 +08:00
|
|
|
CtrlComponents *_ctrlComp;
|
2022-07-20 11:11:38 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
#endif // FSMSTATE_H
|
