62 lines
1.5 KiB
C++
Executable File
62 lines
1.5 KiB
C++
Executable File
#ifndef LOWLEVELSTATE_HPP
|
|
#define LOWLEVELSTATE_HPP
|
|
|
|
#include <iostream>
|
|
#include <vector>
|
|
#include "common/math/mathTools.h"
|
|
#include "common/enumClass.h"
|
|
#include "common/math/Filter.h"
|
|
|
|
struct LowlevelState{
|
|
public:
|
|
LowlevelState(double dt);
|
|
~LowlevelState();
|
|
|
|
std::vector<double> q;
|
|
std::vector<double> dq;
|
|
std::vector<double> ddq;
|
|
std::vector<double> tau;
|
|
|
|
std::vector<std::vector<double>> q_data;
|
|
std::vector<std::vector<double>> dq_data;
|
|
std::vector<std::vector<double>> ddq_data;
|
|
std::vector<std::vector<double>> tau_data;
|
|
|
|
std::vector<int> temperature;
|
|
std::vector<uint8_t> errorstate;
|
|
std::vector<uint8_t> isMotorConnected;
|
|
|
|
std::vector<double> qFiltered;
|
|
std::vector<double> dqFiltered;
|
|
std::vector<double> ddqFiltered;
|
|
std::vector<double> tauFiltered;
|
|
|
|
LPFilter *qFilter;
|
|
LPFilter *dqFilter;
|
|
LPFilter *ddqFilter;
|
|
LPFilter *tauFilter;
|
|
|
|
void resizeGripper(double dt);
|
|
void runFilter();
|
|
bool checkError();
|
|
Vec6 getQ();
|
|
Vec6 getQd();
|
|
Vec6 getQdd();
|
|
Vec6 getTau();
|
|
Vec6 getQFiltered();
|
|
Vec6 getQdFiltered();
|
|
Vec6 getQddFiltered();
|
|
Vec6 getTauFiltered();
|
|
double getGripperQ();
|
|
double getGripperQd();
|
|
double getGripperTau();
|
|
double getGripperTauFiltered();
|
|
private:
|
|
size_t _dof = 6;
|
|
int temporatureLimit = 80.0;// centigrade
|
|
std::vector<int> _isMotorConnectedCnt;
|
|
std::vector<bool> _isMotorLostConnection;
|
|
};
|
|
|
|
#endif //LOWLEVELSTATE_HPP
|