58 lines
840 B
C++
58 lines
840 B
C++
#pragma once
|
|
|
|
#include <vector>
|
|
#include <map>
|
|
|
|
class Leg
|
|
{
|
|
public:
|
|
|
|
enum Joint
|
|
{
|
|
Hipp,
|
|
Knee,
|
|
};
|
|
|
|
Leg(uint8_t hippIndex, uint8_t kneeIndex);
|
|
|
|
//! @brief positive trim in deg to center this joint
|
|
void setTrim(Joint, uint8_t angle);
|
|
|
|
//! @brief position in deg
|
|
void setPos(Joint, double angle);
|
|
|
|
private:
|
|
struct JointInfo {
|
|
uint8_t index;
|
|
uint8_t trim;
|
|
double center;
|
|
double 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();
|
|
|
|
}; |