Simple attitude example

.vscode/extensions.json

@@ -3,8 +3,5 @@
     // for the documentation about the extensions.json format
     "recommendations": [
         "platformio.platformio-ide"
-    ],
-    "unwantedRecommendations": [
-        "ms-vscode.cpptools-extension-pack"
     ]
 }

platformio.ini

@@ -10,11 +10,9 @@
 
 [common]
 lib_deps =
-# Note that the GitHub repository must be a valid PlatformIO library i.e. it must 
-# have library.json or library.properties in its root directory
     OSC #master
     Adafruit PWM Servo Driver Library
-    https://github.com/philsson/PS4-esp32
+    https://github.com/aed3/PS4-esp32.git
 
 [env:featheresp32]
 platform = espressif32

src/body.cpp

@@ -1,4 +1,4 @@
-#include "body.h"
+#include "Body.h"
 
 #include "config.h"
 
@@ -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;
 }

src/main.cpp

@@ -36,11 +36,46 @@ void connectWiFi() {
     Serial.println(WiFi.localIP());
 }
 
+// Simple test. We also have to account for hipp angle which makes this a little more complex
 void testRemoteCallback(const IRemote::Output& o) {
-    for (size_t i = 0; i < 6; ++i) {
-        body.legs[i].setPos(Leg::Hipp, double(o.attitude.roll) * pi / 2 );
-        body.legs[i].setPos(Leg::Knee, double(o.attitude.pitch) * pi / 2 );
-        // Serial.printf("CB: %f, %f\n", o.attitude.roll, o.attitude.pitch);
+    struct Limb {
+        float hipp;
+        float knee;
+        float bodyAngle;
+    };
+
+    std::array<Limb, 6> legs;
+
+    // Initialization
+    const float angleBetweenLegs = pi * 2.0f / 6.0f;
+    float bodyAngle = pi / 2.0f;
+    for (auto& leg : legs)
+    {
+        leg.bodyAngle = bodyAngle;
+
+        // Just to initialize
+        leg.knee = 0.0f;
+        leg.hipp = 0.0f;
+
+        bodyAngle -= angleBetweenLegs;
+    }
+
+    // Attitude
+    for (auto& leg : legs)
+    {
+        // For now yaw will not be overriden by anything
+        leg.hipp = -o.attitude.yaw;
+
+        leg.knee += sinf(leg.bodyAngle) * o.attitude.roll;
+        leg.knee += cosf(leg.bodyAngle) * o.attitude.pitch;
+
+        leg.knee -= o.attitude.elevator;
+    }
+
+    // Actuate
+    for (size_t i = 0; i < body.legs.size(); ++i) {
+        body.legs[i].setPos(Leg::Hipp, legs[i].hipp);
+        body.legs[i].setPos(Leg::Knee, legs[i].knee);
     }
 }