22 lines
552 B
C++
22 lines
552 B
C++
 
|
#pragma once
|
||
|
|
#include <Arduino.h>
|
||
|
|
|
||
|
|
struct JointAngles {
|
||
|
|
float coxa_deg;
|
||
|
|
float femur_deg;
|
||
|
|
float tibia_deg;
|
||
|
|
bool valid; // false if the target was unreachable or outside joint limits
|
||
|
|
};
|
||
|
|
|
||
|
|
struct FootTarget {
|
||
|
|
float x, y, z; // body-frame mm, relative to body center
|
||
|
|
};
|
||
|
|
|
||
|
|
class Kinematics {
|
||
|
|
public:
|
||
|
|
// Solves leg i's joint angles for a body-frame foot target.
|
||
|
|
static JointAngles solve(int leg_index, const FootTarget& target);
|
||
|
|
|
||
|
|
// The leg's resting/neutral foot target, in body frame.
|
||
|
|
static FootTarget neutralStance(int leg_index);
|
||
|
|
};
|