52 lines
889 B
C++
52 lines
889 B
C++
#pragma once
|
|
|
|
#include <map>
|
|
#include <vector>
|
|
|
|
class Leg {
|
|
public:
|
|
enum Joint {
|
|
Hipp,
|
|
Knee,
|
|
};
|
|
|
|
Leg(uint8_t hippIndex, uint8_t kneeIndex);
|
|
|
|
//! @brief positive trim in rad to center this joint
|
|
void setTrim(Joint, float angle);
|
|
|
|
//! @brief position in rad
|
|
void setPos(Joint, float angle);
|
|
|
|
private:
|
|
struct JointInfo {
|
|
uint8_t index;
|
|
float trim;
|
|
float center;
|
|
float pos;
|
|
};
|
|
|
|
std::map<Joint, JointInfo> _joints;
|
|
};
|
|
|
|
typedef std::vector<Leg> Legs;
|
|
|
|
class Body {
|
|
public:
|
|
static Body &instance() {
|
|
static Body instance;
|
|
return instance;
|
|
}
|
|
|
|
//! @brief Nescessary to configure the driver
|
|
void init();
|
|
|
|
//! @brief Check if driver has frozen or lost contact/sync
|
|
//! Resets the driver if nescesarry
|
|
void healthCheck();
|
|
|
|
Legs legs;
|
|
|
|
private:
|
|
Body();
|
|
}; |