From c3e072e2d6c5f238782f994ee58d90fcb3909f82 Mon Sep 17 00:00:00 2001
From: Philip Johansson <philsson@gmail.com>
Date: Sun, 3 Jan 2021 23:54:44 +0100
Subject: [PATCH] Simple attitude example

---
 src/main.cpp | 43 +++++++++++++++++++++++++++++++++++++++----
 1 file changed, 39 insertions(+), 4 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 0a929d6..f45638e 100644
--- a/src/main.cpp
+++ b/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);
     }
 }