From 45a816b5997212186773dd00763a52e191bd1083 Mon Sep 17 00:00:00 2001
From: Philip Johansson <philsson@gmail.com>
Date: Sun, 3 Jan 2021 23:05:42 +0100
Subject: [PATCH] Bugfix in centering and trim of joints

---
 src/body.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/body.cpp b/src/body.cpp
index 2d89011..c97b87e 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -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;
 }
\ No newline at end of file