🪄 Major refactoring to use ESP32 template

This includes the change to use https://github.com/runeharlyk/ESP32-rapid-development-template.git as a freeRTos template

include/AsyncJpegStream.h

@@ -0,0 +1,28 @@
+#include <ESPAsyncWebServer.h>
+#include <esp_camera.h>
+
+typedef struct {
+        camera_fb_t * fb;
+        size_t index;
+} camera_frame_t;
+
+#define PART_BOUNDARY "123456789000000000000987654321"
+
+class AsyncJpegStreamResponse: public AsyncAbstractResponse {
+    private:
+        camera_frame_t _frame;
+        size_t _index;
+        size_t _jpg_buf_len;
+        uint8_t * _jpg_buf;
+        uint64_t lastAsyncRequest;
+    public:
+        AsyncJpegStreamResponse();
+        ~AsyncJpegStreamResponse();
+        bool _sourceValid() const {
+            return true;
+        }
+        virtual size_t _fillBuffer(uint8_t *buf, size_t maxLen) override;
+        size_t _content(uint8_t *buffer, size_t maxLen, size_t index);
+};
+
+void streamJpg(AsyncWebServerRequest *request);

include/IK_config.h

@@ -1,42 +0,0 @@
-#ifndef IK_CONFIG_H
-#define IK_CONFIG_H
-
-#include <Arduino.h>
-
-// SERVOS
-#define Servo_Foot     0
-#define Servo_Leg      1
-#define Servo_Shoulder 2
-
-#define LEG_LF  0 // 0, 1, 2
-#define LEG_RF  1 // 3, 4, 5
-#define LEG_LB  2 // 6, 7, 8
-#define LEG_RB  3 // 9, 10, 11
-
-#define L1  60.5  // y Distance between Shoulder Servo and Leg
-#define L2  10    // z Distance between Shoulder Servo and Leg
-#define L3  100.7 // Length of upper leg
-#define L4  118.5 // Length of lower leg
-#define L   207.5 // Distance between front and back servos
-#define W   78    // Distance between left and right shoulder
-
-// predefined calculations
-#define L1L1  3660.25  //L1*L1
-// #define L1L2  3760.25  //L1*L1+L2*L2
-// #define LL12  1210     //2*L1*L2
-#define L3L3  10140.49 //L3*L3
-#define L4L4  14042.25 //L4*L4
-#define LL34  23865.9  //2*L3*L4
-
-#define SERVO_STEP_ANGLE 2
-#define MOTION_STEP_ANGLE 5
-#define MOTION_STEP_MOVEMENT 5
-#define MOTION_STEP_ALFA 0.20
-
-extern const int16_t servo_min[12] ;
-extern const float servo_conversion[12] ;
-extern const float theta_range[3][2];
-extern const int8_t servo_invert[12];
-extern int16_t servo_angles[4][3];
-
-#endif

include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

include/camera.h

@@ -0,0 +1,3 @@
+#pragma once
+
+esp_err_t InitializeCamera();

include/deviceconfig.h

@@ -0,0 +1,151 @@
+#pragma once
+
+#include <memory>
+#include <types.h>
+#include <jsonserializer.h>
+
+/*
+ * I2C software connection 
+ */
+#define SDA 14
+#define SCL 15
+
+/*
+ * Serial settings
+ */
+#define BAUDRATE 115200
+#define SERIAL_DEBUG_OUTPUT true
+
+/*
+ * PWM controller settings
+ */
+#define SERVO_OSCILLATOR_FREQUENCY 27000000
+#define SERVO_FREQ 50
+
+/*
+ * Button settings
+ */
+#define BUTTON 4
+#define BUTTON_LED 2
+
+/*
+ * Ultra sonic sensors
+ */
+#define USS_LEFT 12
+#define USS_RIGHT 13
+#define USS_MAX_DISTANCE 200
+
+/*
+ * Changeable default data 
+ */
+#define DEVICE_CONFIG_FILE "/device.cfg"
+#define DEFAULT_NTP_SERVER "0.pool.ntp.org"
+
+class DeviceConfig : public IJSONSerializable
+{
+    // Add variables for additional settings to this list
+    String ntpServer;
+    bool useMetric;
+
+    std::vector<SettingSpec> settingSpecs;
+    size_t writerIndex;
+
+    void SaveToJSON();
+
+    template <typename T>
+    void SetAndSave(T& target, const T& source)
+    {
+        if (target == source)
+            return;
+
+        target = source;
+
+        SaveToJSON();
+    }
+
+    template <typename T>
+    void SetIfPresentIn(const JsonObjectConst& jsonObject, T& target, const char *tag)
+    {
+        if (jsonObject.containsKey(tag))
+            target = jsonObject[tag].as<T>();
+    }
+
+  public:
+
+    using ValidateResponse = std::pair<bool, String>;
+
+    // Add additional setting Tags to this list
+    static constexpr const char * NTPServerTag = NAME_OF(ntpServer);
+    static constexpr const char * UseMetricTag = NAME_OF(useMetric);
+
+    DeviceConfig();
+
+    virtual bool SerializeToJSON(JsonObject& jsonObject) override
+    {
+        return SerializeToJSON(jsonObject, true);
+    }
+
+    bool SerializeToJSON(JsonObject& jsonObject, bool includeSensitive)
+    {
+        AllocatedJsonDocument jsonDoc(1024);
+
+        // Add serialization logic for additionl settings to this code
+        jsonDoc[NTPServerTag] = ntpServer;
+        jsonDoc[UseMetricTag] = useMetric;
+
+        return jsonObject.set(jsonDoc.as<JsonObjectConst>());
+    }
+
+    virtual bool DeserializeFromJSON(const JsonObjectConst& jsonObject) override
+    {
+        return DeserializeFromJSON(jsonObject, false);
+    }
+
+    bool DeserializeFromJSON(const JsonObjectConst& jsonObject, bool skipWrite)
+    {
+        // Add deserialization logic for additional settings to this code
+        SetIfPresentIn(jsonObject, ntpServer, NTPServerTag);
+        SetIfPresentIn(jsonObject, useMetric, UseMetricTag);
+
+        if (ntpServer.isEmpty())
+            ntpServer = DEFAULT_NTP_SERVER;
+
+        if (!skipWrite)
+            SaveToJSON();
+
+        return true;
+    }
+
+    void RemovePersisted()
+    {
+        RemoveJSONFile(DEVICE_CONFIG_FILE);
+    }
+
+    virtual const std::vector<SettingSpec>& GetSettingSpecs() const
+    {
+        return settingSpecs;
+    }
+
+    const String &GetNTPServer() const
+    {
+        return ntpServer;
+    }
+
+    void SetNTPServer(const String &newNTPServer)
+    {
+        SetAndSave(ntpServer, newNTPServer);
+    }
+
+    bool UseMetric() const
+    {
+        return useMetric;
+    }
+
+    void SetUseCelsius(bool newUseMetric)
+    {
+        SetAndSave(useMetric, newUseMetric);
+    }
+};
+
+
+extern DRAM_ATTR std::unique_ptr<DeviceConfig> g_ptrDeviceConfig;

include/featureflags.h

@@ -0,0 +1,31 @@
+#define USE_PSRAM true
+
+#define USE_WIFI true
+
+#define WAIT_FOR_WIFI false
+
+#define USE_WEBSERVER true
+
+#define USE_WEBSERVER_SSL false
+
+#define USE_WEBSOCKET true
+
+#define USE_OAT false
+
+#define USE_NTP false
+
+#define USE_MDNS true
+
+#define USE_DNS_SERVER false
+
+#define USE_REMOTE_SERIAL false
+
+#define USE_LOW_POWER false
+
+#define USE_CAMERA true
+
+#define USE_MPU true
+
+#define USE_POWER_BUTTON true
+
+#define USE_USS true

include/globals.h

@@ -0,0 +1,85 @@
+#pragma once
+
+#include <SPIFFS.h>
+#include <Wire.h>
+#include <Adafruit_ADS1X15.h>
+#include <NewPing.h>
+
+// Disable brownout problems
+#include "soc/rtc_cntl_reg.h"
+#include "soc/soc.h"     
+
+/*
+ * Macros
+ */
+#define NAME_OF(x)          #x
+
+/*
+ * Feature flags
+ */
+
+#include <featureflags.h>
+
+#if USE_WIFI
+    #include <WiFi.h>
+#endif
+
+#if USE_WIFI && USE_WEBSERVER
+    #if USE_WEBSERVER_SSL
+        #define ASYNC_TCP_SSL_ENABLED 1
+        #include <ESPAsyncTCP.h>
+    #endif
+    #include <ESPAsyncWebServer.h>
+#endif
+
+#if USE_OAT
+    #include <ArduinoOTA.h>
+#endif
+
+#if USE_DNS_SERVER
+    #include <DNSServer.h>
+#endif
+
+#if USE_MDNS
+    #include <ESPmDNS.h>
+#endif
+
+#define STACK_SIZE (ESP_TASK_MAIN_STACK) // Stack size for each new thread
+
+/*
+ * Thread priority
+ */
+#define NET_PRIORITY            tskIDLE_PRIORITY+5
+#define MOVEMENT_PRIORITY       tskIDLE_PRIORITY+3
+#define JSONWRITER_PRIORITY     tskIDLE_PRIORITY+2
+
+
+
+/*
+ * Thread core
+ */
+#define NET_CORE                1
+#define MOVEMENT_CORE           0
+#define JSONWRITER_CORE         0
+
+
+/*
+ * Main include
+ */
+#include <taskmanager.h>
+#include <movement.h>
+#include <secrets.h>
+#include <servo.h>
+
+#if USE_CAMERA
+
+    #include <camera.h>
+#endif
+
+#if USE_WIFI && USE_WEBSERVER
+    #include <webserver.h>
+#endif
+
+#if USE_WIFI
+    #include <network.h>
+#endif

include/ik_task.h

@@ -1,27 +0,0 @@
-#ifndef IK_TASK_H
-#define IK_TASK_H
-
-#include <Arduino.h>
-#include <servo.h>
-#include <IK_config.h>
-#include <spot_ik.h>
-
-#define DEGREES2RAD 0.017453292519943
-
-typedef  struct {
-    float omega;
-    float phi;
-    float psi;
-    float xm;
-    float ym;
-    float zm;
-    bool set;
-} position_t;
-
-void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm);
-
-void reset_position();
-
-void task_ik(void *ignore);
-
-#endif

include/jsonbase.h

@@ -0,0 +1,4 @@
+#pragma once
+
+#define JSON_BUFFER_BASE_SIZE 2048
+#define JSON_BUFFER_INCREMENT 2048

include/jsonserializer.h

@@ -0,0 +1,80 @@
+#pragma once
+
+#include <atomic>
+#include <ArduinoJson.h>
+#include <jsonbase.h>
+
+struct IJSONSerializable
+{
+    virtual bool SerializeToJSON(JsonObject& jsonObject) = 0;
+    virtual bool DeserializeFromJSON(const JsonObjectConst& jsonObject) { return false; }
+};
+
+template <class E>
+constexpr auto to_value(E e) noexcept
+{
+	return static_cast<std::underlying_type_t<E>>(e);
+}
+
+#if USE_PSRAM
+    struct JsonPsramAllocator
+    {
+        void* allocate(size_t size) {
+            return ps_malloc(size);
+        }
+
+        void deallocate(void* pointer) {
+            free(pointer);
+        }
+
+        void* reallocate(void* ptr, size_t new_size) {
+            return ps_realloc(ptr, new_size);
+        }
+    };
+
+    typedef BasicJsonDocument<JsonPsramAllocator> AllocatedJsonDocument;
+
+#else
+    typedef DynamicJsonDocument AllocatedJsonDocument;
+#endif
+
+bool LoadJSONFile(const char *fileName, size_t& bufferSize, std::unique_ptr<AllocatedJsonDocument>& pJsonDoc);
+bool SaveToJSONFile(const char *fileName, size_t& bufferSize, IJSONSerializable& object);
+bool RemoveJSONFile(const char *fileName);
+
+#define JSON_WRITER_DELAY 3000
+
+class JSONWriter
+{
+    // We allow the main JSON Writer task entry point function to access private members
+    friend void IRAM_ATTR JSONWriterTaskEntry(void *);
+
+  private:
+
+    // Writer function and flag combo
+    struct WriterEntry
+    {
+        std::atomic_bool flag = false;
+        std::function<void()> writer;
+
+        WriterEntry(std::function<void()> writer) :
+            writer(writer)
+        {}
+
+        WriterEntry(WriterEntry&& entry) : WriterEntry(entry.writer)
+        {}
+    };
+
+    std::vector<WriterEntry> writers;
+    std::atomic_ulong latestFlagMs;
+
+  public:
+
+    // Add a writer to the collection. Returns the index of the added writer, for use with FlagWriter()
+    size_t RegisterWriter(std::function<void()> writer);
+
+    // Flag a writer for invocation and wake up the task that calls them
+    void FlagWriter(size_t index);
+};
+
+extern DRAM_ATTR std::unique_ptr<JSONWriter> g_ptrJSONWriter;

include/movement.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include <globals.h>
+
+float getHeading();
+
+float getTemp();
+
+float getAngleX();
+
+float getAngleY();
+
+float getAngleZ();
+
+void IRAM_ATTR MovementHandlingLoopEntry(void *);

include/network.h

@@ -0,0 +1,5 @@
+#pragma once
+
+#include <globals.h>
+
+void IRAM_ATTR NetworkHandlingLoopEntry(void *);

include/servo.h

@@ -1,16 +1,114 @@
-#ifndef SERVO_H
-#define SERVO_H
+#pragma once
 
-#include <esp_err.h>
+#include <memory>
+#include <Adafruit_PWMServoDriver.h>
+#include <globals.h>
+#include <webserver.h>
 
-typedef struct {
-  uint16_t pulse_0; 
-  uint16_t pulse_180; 
-  int8_t invert;
-} servo_settings_t;
+#if USE_WIFI && USE_WEBSERVER
+    extern DRAM_ATTR CWebServer g_WebServer;
+#endif
 
-esp_err_t disable_servos();
-esp_err_t setup_pwm_controller();
-esp_err_t set_servo(uint8_t id, uint16_t angle);
+typedef  struct {
+    float omega;
+    float phi;
+    float psi;
+    float xm;
+    float ym;
+    float zm;
+    bool set;
+} position_t;
 
-#endif
+class Servo : public Adafruit_PWMServoDriver {
+  public:
+    
+    Servo() : Adafruit_PWMServoDriver() {}
+
+    void SetAngles(int8_t* angle) {
+        for(size_t i = 0; i < 12; i++)
+            servo_angles[i] = angle[i];
+        updateServos();
+    }
+
+    void SetAngle(uint8_t id, int8_t angle) {
+        servo_angles[id] = angle;
+        updateServos();
+    }
+
+    void updateServos() {
+        for(uint8_t i = 0; i < 12; i++){
+            int8_t angle = servo_angles[i];
+            uint16_t pulse = (uint16_t) (0.5 + servo_min[i] + (((angle * servo_invert[i]) + 90) * servo_conversion[i]));
+            setPWM(i, 0, pulse);
+        }
+        broadcastAngles();
+    }
+
+    void setBody(float phi, float theta, float psi, float x, float y, float z) {
+        goal_position.phi = (phi - 128) / 2;
+        goal_position.omega = (theta - 128) / 2;
+        goal_position.psi = (psi - 128) / 2; 
+        goal_position.xm = (x - 128) / 2;
+        goal_position.ym = (y - 128) / 2;
+        goal_position.zm = (z - 128) / 2;
+        updateAngles();
+    }
+
+    void setBodyAngle(float phi, float theta, float psi) {
+        goal_position.phi = phi;
+        goal_position.omega = theta;
+        goal_position.psi = psi;
+        updateAngles();
+    }
+
+    void setBodyPosition(float x, float y, float z) {
+        goal_position.xm = x;
+        goal_position.ym = y;
+        goal_position.zm = z;
+        updateAngles();
+    }
+
+    void updateAngles() {
+        servo_angles[0] = goal_position.phi;
+        servo_angles[1] = goal_position.omega;
+        servo_angles[2] = goal_position.psi;
+        servo_angles[3] = goal_position.xm;
+        servo_angles[4] = goal_position.ym;
+        servo_angles[5] = goal_position.zm;
+        updateServos();
+        broadcastAngles();
+    }
+
+    void deactivate() {
+        isActive = false;
+        sleep();
+    }
+
+    void activate() {
+        isActive = true;
+        sleep();
+    }
+
+    void toggleState() {
+        isActive ? sleep() : wakeup();
+        isActive = !isActive;
+    }
+
+    bool isActive {true};
+
+  private:
+    void broadcastAngles() {
+        uint8_t* buf = (uint8_t*)&servo_angles;
+        g_WebServer.broadcast(buf, 12);
+    }
+
+    const int16_t servo_min[12] {92,101,129,92,118,125,110,101,125,92,101,125};
+    const int8_t servo_invert[12] = {-1,1,1, -1,-1,-1, 1,1,1, 1,-1,-1};
+    const float servo_conversion[12] {2.2,2.1055555,1.96923,2.2,2.1055555,1.96923,2.2,2.1055555,1.96923,2.2,2.1055555,1.96923};
+
+    position_t spot_position = {.omega=0,.phi=0,.psi=0,.xm=-40,.ym=-170, .zm=0, .set=1};
+    position_t goal_position = {0,};
+    int8_t servo_angles[12]{0,};
+};
+
+extern DRAM_ATTR std::unique_ptr<Servo> g_ptrServo;

include/servo_config.h

@@ -1,93 +0,0 @@
-#ifndef SERVO_CONFIG_H
-#define SERVO_CONFIG_H
-
-//#include "servo.h"
-#include "spot_ik.h"
-
-// RF - Right Front Leg
-// lower leg
-#define RF_LOWER_SERVO_CHANNEL 8
-#define RF_LOWER_SERVO_CENTER 306
-#define RF_LOWER_SERVO_RANGE 385
-#define RF_LOWER_SERVO_DIRECTION 1
-#define RF_LOWER_SERVO_CENTER_ANG_DEG 99.86f
-// upper leg
-#define RF_UPPER_SERVO_CHANNEL 7
-#define RF_UPPER_SERVO_CENTER 306
-#define RF_UPPER_SERVO_RANGE 407
-#define RF_UPPER_SERVO_DIRECTION 1
-#define RF_UPPER_SERVO_CENTER_ANG_DEG -31.62f
-// shoulder joint
-#define RF_HIP_SERVO_CHANNEL 6
-#define RF_HIP_SERVO_CENTER 306
-#define RF_HIP_SERVO_RANGE 396
-#define RF_HIP_SERVO_DIRECTION -1
-#define RF_HIP_SERVO_CENTER_ANG_DEG 1.67f
-
-// RB - Right Back Leg
-// lower leg
-#define RB_LOWER_SERVO_CHANNEL 2
-#define RB_LOWER_SERVO_CENTER 306
-#define RB_LOWER_SERVO_RANGE 369
-#define RB_LOWER_SERVO_DIRECTION 1
-#define RB_LOWER_SERVO_CENTER_ANG_DEG 95.37f
-// upper leg
-#define RB_UPPER_SERVO_CHANNEL 1
-#define RB_UPPER_SERVO_CENTER 306
-#define RB_UPPER_SERVO_RANGE 381
-#define RB_UPPER_SERVO_DIRECTION 1
-#define RB_UPPER_SERVO_CENTER_ANG_DEG -37.21f
-// shoulder joint
-#define RB_HIP_SERVO_CHANNEL 0
-#define RB_HIP_SERVO_CENTER 306
-#define RB_HIP_SERVO_RANGE 403
-#define RB_HIP_SERVO_DIRECTION 1
-#define RB_HIP_SERVO_CENTER_ANG_DEG -3.27f
-
-// LB - Left Back Leg
-// lower leg
-#define LB_LOWER_SERVO_CHANNEL 5
-#define LB_LOWER_SERVO_CENTER 306
-#define LB_LOWER_SERVO_RANGE 374
-#define LB_LOWER_SERVO_DIRECTION 1
-#define LB_LOWER_SERVO_CENTER_ANG_DEG -92.65f
-// upper leg
-#define LB_UPPER_SERVO_CHANNEL 4
-#define LB_UPPER_SERVO_CENTER 306
-#define LB_UPPER_SERVO_RANGE 403
-#define LB_UPPER_SERVO_DIRECTION 1
-#define LB_UPPER_SERVO_CENTER_ANG_DEG 91.23f
-// shoulder joint
-#define LB_HIP_SERVO_CHANNEL 3
-#define LB_HIP_SERVO_CENTER 306
-#define LB_HIP_SERVO_RANGE 367
-#define LB_HIP_SERVO_DIRECTION -1
-#define LB_HIP_SERVO_CENTER_ANG_DEG -7.20f
-
-// Lf - Left fRONT Leg
-// lower leg
-#define LF_LOWER_SERVO_CHANNEL 11
-#define LF_LOWER_SERVO_CENTER 306
-#define LF_LOWER_SERVO_RANGE 385
-#define LF_LOWER_SERVO_DIRECTION 1
-#define LF_LOWER_SERVO_CENTER_ANG_DEG -87.43f
-// upper leg
-#define LF_UPPER_SERVO_CHANNEL 10
-#define LF_UPPER_SERVO_CENTER 306
-#define LF_UPPER_SERVO_RANGE 388
-#define LF_UPPER_SERVO_DIRECTION 1
-#define LF_UPPER_SERVO_CENTER_ANG_DEG 38.21f
-// shoulder joint
-#define LF_HIP_SERVO_CHANNEL 9
-#define LF_HIP_SERVO_CENTER 306
-#define LF_HIP_SERVO_RANGE 388
-#define LF_HIP_SERVO_DIRECTION 1
-#define LF_HIP_SERVO_CENTER_ANG_DEG 4.67f
-
-extern const int16_t servo_min[12] ;
-extern const float servo_conversion[12] ;
-extern const float theta_range[3][2];
-extern const int8_t servo_invert[12];
-extern int16_t servo_angles[4][3];
-
-#endif

include/spot.h

@@ -1,79 +0,0 @@
-#ifndef SPOT_h
-#define SPOT_h
-
-#include <SPI.h>
-#include <WiFi.h>
-#include <SPIFFS.h>
-#include <NewPing.h>
-#include <ESPmDNS.h>
-#include <AsyncTCP.h>
-#include <DNSServer.h>
-#include <ArduinoOTA.h>
-#include <Adafruit_GFX.h>
-#include <MPU6050_light.h>
-#include <Adafruit_SSD1306.h>
-#include <ESPAsyncWebServer.h>
-#include <Adafruit_PWMServoDriver.h>
-
-// Server functions
-#include <AsyncJpegStreamHandler.h>
-#include <WebsocketHandler.h>
-
-// Disable brownout problems
-#include "soc/rtc_cntl_reg.h"
-#include "soc/soc.h"           
-
-// Config
-#include <config.h>
-#include <camera_pins.h>
-
-#ifdef __cplusplus
-  extern "C" {
-    #endif
-    uint8_t temprature_sens_read();
-    #ifdef __cplusplus
-  }
-#endif
-uint8_t temprature_sens_read();
-
-class Spot {
-    public:
-        Spot();
-
-        esp_err_t boot();
-        void handle();
-
-        esp_err_t initialize_wifi();
-
-        uint8_t cpu_temperature();
-
-        esp_err_t broadcast_data();
-    
-    private:
-        esp_err_t _initialize_camera();
-        esp_err_t _initialize_captive_portal();
-        esp_err_t _initialize_arduino_oat();
-        esp_err_t _initialize_wifi_connection();
-        esp_err_t _initialize_server();
-        esp_err_t _initialize_display();
-        esp_err_t _initialize_mpu();
-        esp_err_t _initialize_pwm_controller();
-        esp_err_t _initialize_button();
-
-        DNSServer _dnsServer;
-        AsyncEventSource _events;
-        AsyncWebSocket _ws;
-        AsyncWebServer _server;
-
-        Adafruit_SSD1306 _display;
-        Adafruit_PWMServoDriver _pwm;
-        MPU6050 _mpu;
-        NewPing _leftUss;
-        NewPing _rightUss;
-
-        unsigned long _last_broadcast{0};
-};
-
-void display_ip_and_ssid(Adafruit_SSD1306* display, String ip, const char* ssid);
-
-#endif

include/spot_ik.h

@@ -1,19 +0,0 @@
-#ifndef SPOT_IK_H
-#define SPOT_IK_H
-
-#include "esp_err.h"
-
-typedef struct {
-    float x;
-    float y;
-    float z;
-} point;
-
-esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t servo_angles[3]);
-esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm);
-esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]);
-
-void print_matrix(float * matrix, int n, int m, char* name) ;
-void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines);
-
-#endif

include/taskmanager.h

@@ -0,0 +1,215 @@
+#pragma once
+
+#include <esp_task_wdt.h>
+
+// Stack size for the taskmgr's idle threads
+#define IDLE_STACK_SIZE 2048
+#define DEFAULT_STACK_SIZE 2048+512
+
+class IdleTask
+{
+  private:
+
+    float _idleRatio = 0;
+    unsigned long _lastMeasurement;
+
+    const int kMillisPerLoop = 1;
+    const int kMillisPerCalc = 1000;
+
+    unsigned long counter = 0;
+
+  public:
+
+    void ProcessIdleTime()
+    {
+        _lastMeasurement = millis();
+        counter = 0;
+
+        // We need to whack the watchdog so we delay in smalle bites until we've used up all the time
+
+        while (true)
+        {
+            int delta = millis() - _lastMeasurement;
+            if (delta >= kMillisPerCalc)
+            {
+                //Serial.printf("Core %u Spent %lu in delay during a window of %d for a ratio of %f\n",
+                //  xPortGetCoreID(), counter, delta, (float)counter/delta);
+                _idleRatio = ((float) counter  / delta);
+                _lastMeasurement = millis();
+                counter = 0;
+            }
+            else
+            {
+                esp_task_wdt_reset();
+                delayMicroseconds(kMillisPerLoop*1000);
+                counter += kMillisPerLoop;
+            }
+        }
+    }
+
+    // If idle time is spent elsewhere, it can be credited to this task.  Shouldn't add up to more time than actual though!
+
+    void CountBonusIdleMillis(uint millis)
+    {
+        counter += millis;
+    }
+
+    IdleTask() : _lastMeasurement(millis())
+    {
+    }
+
+    // GetCPUUsage
+    //
+    // Returns 100 less the amount of idle time that we were able to squander.
+
+    float GetCPUUsage() const
+    {
+        // If the measurement failed to even get a chance to run, this core is maxed and there was no idle time
+
+        if (millis() - _lastMeasurement > kMillisPerCalc)
+            return 100.0f;
+
+        // Otherwise, whatever cycles we were able to burn in the idle loop counts as "would have been idle" time
+        return 100.0f-100*_idleRatio;
+    }
+
+    // Stub entry point for calling into it without a THIS pointer
+
+    static void IdleTaskEntry(void * that)
+    {
+        IdleTask * pTask = (IdleTask *)that;
+        pTask->ProcessIdleTime();
+    }
+};
+
+// TaskManager
+//
+// TaskManager runs two tasks at just over idle priority that do nothing but try to burn CPU, and they
+// keep track of how much they can burn.   It's assumed that everything else runs at a higher priority
+// and thus they "starve" the idle tasks when doing work.
+
+class TaskManager
+{
+    TaskHandle_t _hIdle0 = nullptr;
+    TaskHandle_t _hIdle1 = nullptr;
+
+    IdleTask _taskIdle0;
+    IdleTask _taskIdle1;
+
+  public:
+
+    float GetCPUUsagePercent(int iCore = -1) const
+    {
+        if (iCore < 0)
+            return (_taskIdle0.GetCPUUsage() + _taskIdle1.GetCPUUsage()) / 2;
+        else if (iCore == 0)
+            return _taskIdle0.GetCPUUsage();
+        else if (iCore == 1)
+            return _taskIdle1.GetCPUUsage();
+        else
+            throw new std::runtime_error("Invalid core passed to GetCPUUsagePercentCPU");
+    }
+
+    TaskManager() {}
+
+    void begin()
+    {
+        Serial.printf("Replacing Idle Tasks with TaskManager...\n");
+        // The idle tasks get created with a priority just ABOVE idle so that they steal idle time but nothing else.  They then
+        // measure how much time is "wasted" at that lower priority and deem it to have been free CPU
+
+        xTaskCreatePinnedToCore(_taskIdle0.IdleTaskEntry, "Idle0", IDLE_STACK_SIZE, &_taskIdle0, tskIDLE_PRIORITY + 1, &_hIdle0, 0);
+        xTaskCreatePinnedToCore(_taskIdle1.IdleTaskEntry, "Idle1", IDLE_STACK_SIZE, &_taskIdle1, tskIDLE_PRIORITY + 1, &_hIdle1, 1);
+
+        // We need to turn off the watchdogs because our idle measurement tasks burn all of the idle time just
+        // to see how much there is (it's how they measure free CPU).  Thus, we starve the system's normal idle tasks
+        // and have to feed the watchdog on our own.
+
+        esp_task_wdt_delete(xTaskGetIdleTaskHandleForCPU(0));
+        esp_task_wdt_delete(xTaskGetIdleTaskHandleForCPU(1));
+        esp_task_wdt_add(_hIdle0);
+        esp_task_wdt_add(_hIdle1);
+    }
+};
+
+void IRAM_ATTR NetworkHandlingLoopEntry(void *);
+void IRAM_ATTR JSONWriterTaskEntry(void *);
+void IRAM_ATTR MovementHandlingLoopEntry(void *);
+
+#define DELETE_TASK(handle) if (handle != nullptr) vTaskDelete(handle)
+
+class ESPTaskManager : public TaskManager
+{
+public:
+
+private:
+    TaskHandle_t _taskNetwork       = nullptr;
+    TaskHandle_t _taskMovement      = nullptr;
+    TaskHandle_t _taskJSONWriter    = nullptr;
+
+public:
+
+    ~ESPTaskManager()
+    {
+        DELETE_TASK(_taskNetwork);
+        DELETE_TASK(_taskMovement);
+        DELETE_TASK(_taskJSONWriter);
+    }
+
+    void StartThreads(){
+        StartNetworkThread();
+        StartMovementThread();
+        StartJSONWriterThread();
+    }
+
+    void StartNetworkThread()
+    {
+        #if USE_WIFI
+            log_i( ">> Launching Network Thread.  Mem: %u, LargestBlk: %u, PSRAM Free: %u/%u, ", ESP.getFreeHeap(),ESP.getMaxAllocHeap(), ESP.getFreePsram(), ESP.getPsramSize());
+            xTaskCreatePinnedToCore(NetworkHandlingLoopEntry, "NetworkHandlingLoop", STACK_SIZE, nullptr, NET_PRIORITY, &_taskNetwork, NET_CORE);
+        #endif
+    }
+
+    void StartMovementThread()
+    {
+        log_i(">> Launching Movement Thread");
+        xTaskCreatePinnedToCore(MovementHandlingLoopEntry, "MovementHandlingLoop", STACK_SIZE, nullptr, MOVEMENT_PRIORITY, &_taskMovement, MOVEMENT_CORE);
+    }
+
+    void StartJSONWriterThread()
+    {
+        log_i(">> Launching JSON Writer Thread");
+        xTaskCreatePinnedToCore(JSONWriterTaskEntry, "JSON Writer Loop", STACK_SIZE, nullptr, JSONWRITER_PRIORITY, &_taskJSONWriter, JSONWRITER_CORE);
+    }
+
+    void NotifyJSONWriterThread()
+    {
+        if (_taskJSONWriter == nullptr)
+            return;
+
+        log_w(">> Notifying JSON Writer Thread");
+        // Wake up the writer invoker task if it's sleeping, or request another write cycle if it isn't
+        xTaskNotifyGive(_taskJSONWriter);
+    }
+
+    void NotifyNetworkThread()
+    {
+        if (_taskNetwork == nullptr)
+            return;
+
+        //debugW(">> Notifying Network Thread");
+        // Wake up the network task if it's sleeping, or request another read cycle if it isn't
+        xTaskNotifyGive(_taskNetwork);
+    }
+
+    void NotifyMovementThread()
+    {
+        if (_taskMovement == nullptr)
+            return;
+
+        // Wake up the movement task if it's sleeping, or request another read cycle if it isn't
+        xTaskNotifyGive(_taskMovement);
+    }
+};
+
+extern ESPTaskManager g_TaskManager;

include/types.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <cstddef>
+#include <cstdint>
+#include <WString.h>
+
+struct EmbeddedFile
+{
+    // Embedded file size in bytes
+    const size_t length;
+    // Contents as bytes
+    const uint8_t *const contents;
+
+    EmbeddedFile(const uint8_t start[], const uint8_t end[]) :
+        length(end - start),
+        contents(start)
+    {}
+};
+
+struct SettingSpec
+{
+    // Note that if this enum is expanded, ToName() must be also!
+    enum class SettingType : int
+    {
+        Integer,
+        PositiveBigInteger,
+        Float,
+        Boolean,
+        String,
+        Palette
+    };
+
+    String Name;
+    String FriendlyName;
+    String Description;
+    SettingType Type;
+
+    SettingSpec(const String& name, const String& friendlyName, const String& description, SettingType type)
+      : Name(name),
+        FriendlyName(friendlyName),
+        Description(description),
+        Type(type)
+    {}
+
+    SettingSpec(const String& name, const String& friendlyName, SettingType type) : SettingSpec(name, friendlyName, "", type)
+    {}
+
+    SettingSpec()
+    {}
+
+    String static ToName(SettingType type)
+    {
+        String names[] = { "Integer", "PositiveBigInteger", "Float", "Boolean", "String", "Palette" };
+        return names[(int)type];
+    }
+};

include/webserver.h

@@ -0,0 +1,219 @@
+#pragma once
+
+#include <map>
+#include <ESPAsyncWebServer.h>
+#include <deviceconfig.h>
+#include <AsyncJson.h>
+#include <ArduinoJson.h>
+#include <HTTPClient.h>
+#include <Update.h>
+#include <SPIFFS.h>
+#include <servo.h>
+#include <AsyncJpegStream.h>
+
+#define HTTP_CODE_OK 200
+#define HTTP_CODE_NOT_FOUND 404
+
+class CWebServer {
+  private:
+    // Template for param to value converter function, used by PushPostParamIfPresent()
+    template<typename Tv>
+    using ParamValueGetter = std::function<Tv(AsyncWebParameter *param)>;
+
+    // Template for value setting forwarding function, used by PushPostParamIfPresent()
+    template<typename Tv>
+    using ValueSetter = std::function<bool(Tv)>;
+
+    // Value validating function type, as used by DeviceConfig (and possible others)
+    using ValueValidator = std::function<DeviceConfig::ValidateResponse(const String&)>;
+
+    struct StaticStatistics {
+        uint32_t HeapSize;
+        size_t DmaHeapSize;
+        uint32_t PsramSize;
+        const char *ChipModel;
+        uint8_t ChipCores;
+        uint32_t CpuFreqMHz;
+        uint32_t SketchSize;
+        uint32_t FreeSketchSpace;
+        uint32_t FlashChipSize;
+    };
+
+    static std::vector<SettingSpec> deviceSettingSpecs;
+    static const std::map<String, ValueValidator> settingValidators;
+
+    AsyncWebServer _server;
+    StaticStatistics _staticStats;
+    #if USE_WEBSOCKET
+    AsyncWebSocket _ws;
+    #endif
+
+    // Helper functions/templates
+
+    // Convert param value to a specific type and forward it to a setter function that expects that type as an argument
+    template<typename Tv>
+    static bool PushPostParamIfPresent(AsyncWebServerRequest * pRequest, const String &paramName, ValueSetter<Tv> setter, ParamValueGetter<Tv> getter)
+    {
+        if (!pRequest->hasParam(paramName, true, false))
+            return false;
+
+        log_v("found %s", paramName.c_str());
+
+        AsyncWebParameter *param = pRequest->getParam(paramName, true, false);
+
+        // Extract the value and pass it off to the setter
+        return setter(getter(param));
+    }
+
+    // Generic param value forwarder. The type argument must be implicitly convertable from String!
+    //   Some specializations of this are included in the CPP file
+    template<typename Tv>
+    static bool PushPostParamIfPresent(AsyncWebServerRequest * pRequest, const String &paramName, ValueSetter<Tv> setter)
+    {
+        return PushPostParamIfPresent<Tv>(pRequest, paramName, setter, [](AsyncWebParameter * param) { return param->value(); });
+    }
+
+    // AddCORSHeaderAndSend(OK)Response
+    //
+    // Sends a response with CORS headers added
+    template<typename Tr>
+    static void AddCORSHeaderAndSendResponse(AsyncWebServerRequest * pRequest, Tr * pResponse)
+    {
+        // pResponse->addHeader("Server", HOSTNAME);
+        // pResponse->addHeader("Access-Control-Allow-Origin", "*");
+        pRequest->send(pResponse);
+    }
+
+    // Version for empty response, normally used to finish up things that don't return anything, like "NextEffect"
+    static void AddCORSHeaderAndSendOKResponse(AsyncWebServerRequest * pRequest)
+    {
+        AddCORSHeaderAndSendResponse(pRequest, pRequest->beginResponse(HTTP_CODE_OK));
+    }
+
+    // Straightforward support functions
+
+    static bool IsPostParamTrue(AsyncWebServerRequest * pRequest, const String & paramName);
+    static const std::vector<SettingSpec> & LoadDeviceSettingSpecs();
+    static void SendSettingSpecsResponse(AsyncWebServerRequest * pRequest, const std::vector<SettingSpec> & settingSpecs);
+    static void SetSettingsIfPresent(AsyncWebServerRequest * pRequest);
+    #if USE_OAT
+    static void UpdateFirmware(AsyncWebServerRequest *request, String filename, size_t index, uint8_t *data, size_t len, bool final);
+    static void UpdateFileSystemImage(AsyncWebServerRequest *request, String filename, size_t index, uint8_t *data, size_t len, bool final);
+    static void HandleUpdate(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final, size_t upload_size, uint8_t upload_index);
+    #endif
+
+    // Endpoint member functions
+    static void GetSettingSpecs(AsyncWebServerRequest * pRequest);
+    static void GetSettings(AsyncWebServerRequest * pRequest);
+    static void SetSettings(AsyncWebServerRequest * pRequest);
+    static void SaveFile(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final);
+    #if USE_OAT
+    static void UpdateRequestHandler(AsyncWebServerRequest * pRequest);
+    #endif
+    static void Reset(AsyncWebServerRequest * pRequest);
+    static void GzipSpa(AsyncWebServerRequest * pRequest);
+    static void HandleWsMessage(AsyncWebSocket* server, AsyncWebSocketClient* client, AwsEventType type, void* arg, uint8_t* data, size_t len);
+
+    // Not static because it uses member _staticStats
+    void GetStatistics(AsyncWebServerRequest * pRequest);
+
+  public:
+
+    CWebServer()
+        : _server(HTTP_PORT)
+        #if USE_WEBSOCKET
+        , _ws(WEBSOCKET_PATH)
+        #endif
+    {}
+
+    void begin() {
+        DefaultHeaders::Instance().addHeader("Access-Control-Allow-Origin", "*");
+        DefaultHeaders::Instance().addHeader("Access-Control-Allow-Methods", "GET, POST, PUT");
+        DefaultHeaders::Instance().addHeader("Access-Control-Allow-Headers", "Content-Type");
+        DefaultHeaders::Instance().addHeader("Server", HOSTNAME);
+
+        _staticStats.HeapSize = ESP.getHeapSize();
+        _staticStats.DmaHeapSize = heap_caps_get_total_size(MALLOC_CAP_DMA);
+        _staticStats.PsramSize = ESP.getPsramSize();
+        _staticStats.ChipModel = ESP.getChipModel();
+        _staticStats.ChipCores = ESP.getChipCores();
+        _staticStats.CpuFreqMHz = ESP.getCpuFreqMHz();
+        _staticStats.SketchSize = ESP.getSketchSize();
+        _staticStats.FreeSketchSpace = ESP.getFreeSketchSpace();
+        _staticStats.FlashChipSize = ESP.getFlashChipSize();
+
+        _server.onFileUpload(SaveFile);
+
+        #if USE_WEBSOCKET
+        _ws.onEvent(HandleWsMessage);
+        _server.addHandler(&_ws);
+        #endif
+        
+        _server.on("/api/statistics",            HTTP_GET,  [this](AsyncWebServerRequest * pRequest)    { this->GetStatistics(pRequest); });
+        _server.on("/api/getStatistics",         HTTP_GET,  [this](AsyncWebServerRequest * pRequest)    { this->GetStatistics(pRequest); });
+  
+        _server.on("/api/settings/specs",        HTTP_GET,  [](AsyncWebServerRequest * pRequest)        { GetSettingSpecs(pRequest); });
+        _server.on("/api/settings",              HTTP_GET,  [](AsyncWebServerRequest * pRequest)        { GetSettings(pRequest); });
+        _server.on("/api/settings",              HTTP_POST, [](AsyncWebServerRequest * pRequest)        { SetSettings(pRequest); });
+
+        #if USE_OAT
+        _server.on("/api/update/filesystem",     HTTP_POST, UpdateRequestHandler,                       UpdateFirmware);
+        _server.on("/api/update/firmware",       HTTP_POST, UpdateRequestHandler,                       UpdateFileSystemImage);
+        #endif
+
+        _server.on("/api/stream",                HTTP_GET,  streamJpg);
+
+        _server.on("/api/reset",                 HTTP_POST, [](AsyncWebServerRequest * pRequest)        { Reset(pRequest); });
+
+        _server.serveStatic("/", SPIFFS, "/").setCacheControl("max-age=31536000");
+
+        _server.onNotFound([](AsyncWebServerRequest *pRequest) { GzipSpa(pRequest); });
+
+        #if USE_WEBSERVER_SSL
+            _server.beginSecure("/server.cer", "/server.key", NULL);
+
+            _server.onSslFileRequest([](void * arg, const char *filename, uint8_t **buf) -> int {
+                Serial.printf("SSL File: %s\n", filename);
+                File file = SPIFFS.open(filename, "r");
+                if(file){
+                size_t size = file.size();
+                uint8_t * nbuf = (uint8_t*)malloc(size);
+                if(nbuf){
+                    size = file.read(nbuf, size);
+                    file.close();
+                    *buf = nbuf;
+                    return size;
+                }
+                file.close();
+                }
+                *buf = 0;
+                return 0;
+            }, NULL);
+        #else
+            _server.begin();
+        #endif
+
+        log_i("HTTP server started");
+    }
+
+    void loop() {
+        #if USE_WEBSOCKET
+        _ws.cleanupClients();
+        #endif
+    }
+
+    void broadcast(uint8_t* content, size_t length) {
+        _ws.binaryAll(content, length);
+    }
+
+    void broadcastJson(char* content, size_t length) {
+        _ws.textAll(content, length);
+    }
+};
+
+// Set value in lambda using a forwarding function. Always returns true
+#define SET_VALUE(functionCall) [](auto value) { functionCall; return true; }
+
+// Set value in lambda using a forwarding function. Reports success based on function's return value,
+//   which must be implicitly convertable to bool
+#define CONFIRM_VALUE(functionCall) [](auto value)->bool { return functionCall; }