Removed deg for rad and set physical limits

include/IRemote.h

@@ -7,9 +7,9 @@ public:
     //! @brief Attitude is the "pose" of the Hexapod
     //!        without movement
     //!
-    //! @arg roll/pitch   Tilt it around.    Deg [-45, 45]
+    //! @arg roll/pitch   Tilt it around.    [-1, 1]
-    //! @arg yaw          Twist it.          Deg [-45, 45]
+    //! @arg yaw          Twist it.          [-1, 1]
-    //! @arg elevator     Lower or raise it. m [0, 0.1]
+    //! @arg elevator     Lower or raise it. [-1, 1]
     typedef struct {
         float roll;
         float pitch;
@@ -19,9 +19,9 @@ public:
 
     //! @brief Movement consists of the target velocity for
     //!
-    //! @arg vx Vel back/forward. m [-?, ?]
+    //! @arg vx Vel back/forward. [-1, 1]
-    //! @arg vy Vel left/right.   m [-?, ?]
+    //! @arg vy Vel left/right.   [-1, 1]
-    //! @arg w  Rotational speed. deg/s [-20, 20]
+    //! @arg w  Rotational speed. [-1, 1]
     typedef struct {
         float vx;
         float vy;
@@ -36,6 +36,8 @@ public:
 
     virtual const Output& output() = 0;
 
+    virtual void init() = 0;
+    
     virtual void loop() = 0;
 
     virtual void registerCallback(void (*callback)(const Output&)) = 0;

include/body.h

@@ -12,18 +12,18 @@ public:
 
     Leg(uint8_t hippIndex, uint8_t kneeIndex);
 
-    //! @brief positive trim in deg to center this joint
+    //! @brief positive trim in rad to center this joint
-    void setTrim(Joint, uint8_t angle);
+    void setTrim(Joint, float angle);
 
-    //! @brief position in deg
+    //! @brief position in rad
-    void setPos(Joint, double angle);
+    void setPos(Joint, float angle);
 
 private:
     struct JointInfo {
         uint8_t index;
-        uint8_t trim;
+        float trim;
-        double center;
+        float center;
-        double pos;
+        float pos;
     };
 
     std::map<Joint, JointInfo> _joints;

src/body.cpp

@@ -7,6 +7,8 @@
 #include <SPI.h>
 #include <Wire.h>
 
+#include <cmath>
+
 // Depending on your servo make, the pulse width min and max may vary, you
 // want these to be as small/large as possible without hitting the hard stop
 // for max range. You'll have to tweak them as necessary to match the servos you
@@ -63,13 +65,15 @@ void checkForServoSleep() {
 #endif
 }
 
-double modifiedMap(double x, double in_min, double in_max, double out_min, double out_max) {
+float modifiedMap(float x, float in_min, float in_max, float out_min, float out_max) {
-    double temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+    float temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
     temp = (int)(4 * temp + .5);
-    return (double)temp / 4;
+    return (float)temp / 4;
 }
 
-Body::Body() : legs() {
+Body::Body() 
+: legs() 
+{
     uint8_t numOfLegs = 6;
     for (uint8_t i = 0; i < numOfLegs; ++i) {
         legs.push_back(Leg(i, i + 6));
@@ -85,24 +89,22 @@ 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 = 45.0, .pos = 0}}}) {
+               {Joint::Knee, {.index = knee, .trim = 0, .center = pi/4, .pos = 0}}}) {
 }
 
-void Leg::setTrim(Joint j, uint8_t angle) {
+void Leg::setTrim(Joint j, float angle) {
     _joints[j].trim = angle;
 }
 
-void Leg::setPos(Joint j, double angle) {
+void Leg::setPos(Joint j, float angle) {
-    //! @todo Trying to evaluate if one of these has better performance on the
-    //! servo
 
-#ifndef CTRL_INACTIVE
+    const float maxAngle = 1.43117; // both directions (visually observed)
-    // auto angleToPulse = [](double a) { return modifiedMap(a, -180.0, 180.0, SERVOMIN, SERVOMAX);
-    // } servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle));
 
-    auto angleToMicroPulse = [](double a) { return modifiedMap(a, -180.0, 180.0, USMIN, USMAX); };
+    angle = std::min(std::max(angle, -maxAngle), maxAngle);
-    servoDriver.writeMicroseconds(_joints[j].index, angleToMicroPulse(angle) + _joints[j].center);
+
-#endif
+    servoDriver.writeMicroseconds(
+        _joints[j].index, 
+        modifiedMap(angle, -maxAngle, maxAngle, USMIN, USMAX) + _joints[j].center);
 
     _joints[j].pos = angle;
 }

src/main.cpp

@@ -37,23 +37,17 @@ void connectWiFi() {
 
 void testRemoteCallback(const IRemote::Output& o) {
     for (size_t i = 0; i < 6; ++i) {
-        body.legs[i].setPos(Leg::Hipp, double(o.attitude.roll) * 4);
+        body.legs[i].setPos(Leg::Hipp, double(o.attitude.roll) * pi / 2 );
-        body.legs[i].setPos(Leg::Knee, double(o.attitude.pitch) * 4);
+        body.legs[i].setPos(Leg::Knee, double(o.attitude.pitch) * pi / 2 );
         // Serial.printf("CB: %f, %f\n", o.attitude.roll, o.attitude.pitch);
     }
 }
 
+// Just a movement with one leg to show that we have booted
 void sweepLeg() {
-    static uint8_t d = 0;
+    body.legs[1].setPos(Leg::Knee, pi / 2.0f);
-    if (d < 2) {
+    sleep(1);
-        for (long angle = 0; angle < 180 * 5; angle++) {
+    body.legs[1].setPos(Leg::Knee, 0.0f);
-            body.legs[1].setPos(Leg::Knee, double(angle / 5));
-        }
-        for (long angle = 180 * 5; angle > 0; angle--) {
-            body.legs[1].setPos(Leg::Knee, double(angle / 5));
-        }
-    }
-    d++;
 }
 
 void checkConfig() {