philip/Hexapod
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Bugfix in centering and trim of joints

Browse Source
This commit is contained in:
Philip Johansson 2021-01-03 23:05:42 +01:00
parent dceea6bfea
commit 45a816b599
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/body.cpp
View File

@ -89,7 +89,7 @@ void Body::healthCheck() {
Leg::Leg(uint8_t hipp, uint8_t knee)
: _joints({{Joint::Hipp, {.index = hipp, .trim = 0, .center = 0.0, .pos = 0}},
{Joint::Knee, {.index = knee, .trim = 0, .center = pi/4, .pos = 0}}}) {
{Joint::Knee, {.index = knee, .trim = 0, .center = -pi/4, .pos = 0}}}) {
}
void Leg::setTrim(Joint j, float angle) {
@ -100,11 +100,13 @@ void Leg::setPos(Joint j, float angle) {
const float maxAngle = 1.43117; // both directions (visually observed)
angle += (_joints[j].center + _joints[j].trim);
angle = std::min(std::max(angle, -maxAngle), maxAngle);
servoDriver.writeMicroseconds(
_joints[j].index,
modifiedMap(angle, -maxAngle, maxAngle, USMIN, USMAX) + _joints[j].center);
modifiedMap(angle, -maxAngle, maxAngle, USMIN, USMAX));
_joints[j].pos = angle;
}