🪄 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
2023-08-02 22:26:09 +02:00
parent 4c7b8954eb
commit 4282055be0
42 changed files with 2169 additions and 1065 deletions
@@ -3,3 +3,6 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
+data/
+include/secrets.h
+lib/*
@@ -6,10 +6,17 @@
    "editor.detectIndentation": false,
    "cmake.sourceDirectory": "C:/data/repos/Hardware/Spot Micro - Leika/.pio/libdeps/esp32cam/esp32-camera",
    "cSpell.words": [
+        "Adafruit",
+        "IRAM",
+        "Leika",
        "lerp",
+        "MDNS",
+        "Psram",
        "smnc",
+        "ssid",
        "URDF",
        "uzip",
+        "WEBSERVER",
        "xacro"
    ]
 }
@@ -2,7 +2,7 @@
 	import { onDestroy } from 'svelte';
 	import location from '../lib/location';

-	let videoStream = `//${location}/stream`;
+	let videoStream = `//${location}/api/stream`;

 	onDestroy(() => {
 		videoStream = '#';
@@ -18,12 +18,14 @@ import {
 	FogExp2,
 	MeshBasicMaterial,
 	CanvasTexture,
-	CircleGeometry
+	CircleGeometry,
+	Object3D,
+	type Event
 } from 'three';
 import { XacroLoader } from 'xacro-parser';
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
 import URDFLoader from 'urdf-loader';
-import { servoBuffer } from '../../lib/socket'
+import { dataBuffer, servoBuffer } from '../../lib/socket'
 import { lerp } from '../../lib/utils';
 import uzip from 'uzip';
 import { outControllerData } from '../../lib/store';
@@ -31,7 +33,7 @@ import Kinematic from '../../lib/kinematic';
 import location from '../../lib/location';
 import FileCache from '../../lib/cache';

-let canvas: HTMLCanvasElement, streamCanvas: HTMLCanvasElement, stream: HTMLImageElement, scene: Scene, camera: Camera, renderer: WebGLRenderer, controls: OrbitControls, robot, isLoaded = false;
+let canvas: HTMLCanvasElement, streamCanvas: HTMLCanvasElement, stream: HTMLImageElement, scene: Scene, camera: Camera, renderer: WebGLRenderer, controls: OrbitControls, robot: Object3D<Event>, isLoaded = false;

 let context: CanvasRenderingContext2D, texture: CanvasTexture

@@ -45,7 +47,7 @@ let modelBodyPoint:Point = {x: 0, y: 0, z: 0 }
 let modelTargetBodyPoint:Point = {x: 0, y: 0, z: 0 }

 const dir = [ -1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1]
-const videoStream = `//${location}/stream`;
+const videoStream = `//${location}/api/stream`;

 let showModel = true, showStream = false

@@ -223,6 +225,8 @@ const render = () => {

    if(!robot) return

+    robot.rotation.z = lerp(robot.rotation.z, MathUtils.degToRad($dataBuffer[1]), 0.1)
+
    if(!isLoaded){
        const intervalId = setInterval(() => {
            robot.traverse(c => c.castShadow = true);
@@ -31,7 +31,10 @@
                <Icon src={Bars3} size="32" />
            </button>
        </div>
-        <div class="w-20 p-4 text-right">{Math.floor($dataBuffer[5])}°🌡️</div>
+        <div class="w-20 p-4 text-right">{Math.floor($dataBuffer[0])}°🌡️<br>
+            {Math.floor($dataBuffer[9]*10)/10}V<br>
+            {Math.floor($dataBuffer[8]*1000)/1000}A
+        </div>
    </div>
    <div class="absolute flex justify-center w-full">
        <div>
@@ -45,6 +45,8 @@ const _disconnected = () => {
 const _message = (event) => {    
    if (event.data instanceof ArrayBuffer) {
        let buffer = new Int8Array(event.data);
-        servoBuffer.set(buffer);
+        if(buffer.length === 44) {
+            dataBuffer.set(new Float32Array(buffer.buffer) )
+        }
    } else dataBuffer.set(JSON.parse(event.data));
 }
@@ -25,6 +25,8 @@
 <div class="w-full h-full absolute top-0 p-4 flex justify-center items-center">
    <Card class="w-full h-1/2">
        <CardHeader>Servo calibration</CardHeader>
+        <div>Servo</div>
+        <input type="number" class="bg-background" bind:value={servo}>
        <div class="flex w-full">
            <label for="pwm">Pwm ({pwm})</label>
            <div class="flex w-full">
@@ -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
@@ -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
@@ -0,0 +1,3 @@
+#pragma once
+
+esp_err_t InitializeCamera();
@@ -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;
@@ -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
@@ -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
@@ -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
@@ -0,0 +1,4 @@
+#pragma once
+
+#define JSON_BUFFER_BASE_SIZE 2048
+#define JSON_BUFFER_INCREMENT 2048
@@ -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;
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <globals.h>
+
+float getHeading();
+
+float getTemp();
+
+float getAngleX();
+
+float getAngleY();
+
+float getAngleZ();
+
+void IRAM_ATTR MovementHandlingLoopEntry(void *);
@@ -0,0 +1,5 @@
+#pragma once
+
+#include <globals.h>
+
+void IRAM_ATTR NetworkHandlingLoopEntry(void *);
@@ -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;
@@ -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
@@ -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
@@ -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
@@ -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;
@@ -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];
+    }
+};
@@ -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; }
@@ -1,15 +0,0 @@
-#include <ESPAsyncWebServer.h>
-#include "ArduinoJson.h"
-
-extern uint8_t inputData[6];
-
-struct websocket_message {
-    bool is_handled;
-    uint8_t identifier;
-    uint8_t* data;
-    size_t len;
-};
-
-extern websocket_message wsm;
-
-void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventType type, void * arg, uint8_t *data, size_t len);
@@ -1,75 +0,0 @@
-#include <WebsocketHandler.h>
-
-uint8_t inputData[6] = {};
-uint16_t servoData[12] = {};
-
-esp_err_t parseControllerPacket(uint8_t* data, uint64_t lenght){
-  log_i("parsing controller packet");
-  for(size_t i=0; i < lenght; i++) {
-    inputData[i] = (uint8_t) data[i];
-  }
-  return ESP_OK;
-}
-
-esp_err_t parseServoPacket(uint8_t* _data, uint64_t lenght){
-  log_i("parsing servo packet");
-  uint16_t* data = (uint16_t*)_data;
-  for(size_t i=0; i < lenght/2; i++) {
-    servoData[i] = data[i];
-  }
-  log_i("done parsing servo packet. There was %u int", lenght);
-  return ESP_OK;
-}
-
-enum data_packet_t {
-  CONTROLLER_PACKET,
-  SERVO_PACKET,
-  toggle,
-};
-
-void handleWebSocketBufferMessage(void* arg, uint8_t* data, size_t len, AsyncWebSocket* server, AsyncWebSocketClient* client) {
-  wsm.identifier = data[0];
-  wsm.data = data+1;
-  wsm.len = len;
-  wsm.is_handled = false;
-}
-
-void handleWebSocketMessage(void* arg, uint8_t* data, size_t len, AsyncWebSocket* server, AsyncWebSocketClient* client) {
-    AwsFrameInfo* info = (AwsFrameInfo*)arg;
-    if(info->final && info->index == 0 && info->len == len){
-        if(info->opcode == WS_TEXT) return;
-        handleWebSocketBufferMessage(arg, data, len, server, client);
-    }
-}
-
-void handleNewConnection(AsyncWebSocket* server, AsyncWebSocketClient* client){
-    log_i("ws[%s][%u] connect\n", server->url(), client->id());
-
-    StaticJsonDocument<600> json;
-    json["ssid"] = "Rune private network";
-    json["password"] = "a9b8c7d6e5f4g3H2I1";
-    json["sketchSize"] = ESP.getSketchSize();
-    json["sketchMD5"] = ESP.getSketchMD5();
-    json["freeSketchSize"] = ESP.getFreeSketchSpace();
-    json["chipCores"] = ESP.getChipCores();
-    json["chipModel"] = ESP.getChipModel();
-    json["chipRevision"] = ESP.getChipRevision();
-    json["cpuFreq"] = ESP.getCpuFreqMHz();
-    json["heapSize"] = ESP.getHeapSize();
-    json["psramSize"] = ESP.getPsramSize();
-    json["sdkVersion"] = ESP.getSdkVersion();
-    json["eFuseMac"] = ESP.getEfuseMac();
-    json["resetReason"] = esp_reset_reason();
-
-    char data[400];
-    size_t len = serializeJson(json, data);
-    client->text(data, len);
-}
-
-void onWsEvent(AsyncWebSocket* server, AsyncWebSocketClient* client, AwsEventType type, void* arg, uint8_t* data, size_t len) {
-    if (type == WS_EVT_CONNECT) handleNewConnection(server, client);
-    else if (type == WS_EVT_DISCONNECT) log_i("ws[%s][%u] disconnect\n", server->url(), client->id());
-    else if (type == WS_EVT_ERROR) log_i("ws[%s][%u] error(%u): %s\n", server->url(), client->id(), *((uint16_t*)arg), (char*)data);
-    else if (type == WS_EVT_PONG) log_i("ws[%s][%u] pong[%u]: %s\n", server->url(), client->id(), len, (len) ? (char*)data : "");
-    else if (type == WS_EVT_DATA) handleWebSocketMessage(arg, data, len, server, client);
-}
@@ -8,19 +8,33 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html

-[env:esp32cam]
+[base]
 platform = espressif32
-board = esp32cam
-monitor_speed = 115200
 framework = arduino
+monitor_speed = 115200
+monitor_filters = esp32_exception_decoder
 build_flags = 
 	-DCORE_DEBUG_LEVEL=3
+	-std=gnu++17
+	-Dregister=
+build_unflags = -std=gnu++11
+test_ignore = test_embedded
 lib_deps = 
-	https://github.com/Aircoookie/ESPAsyncWebServer.git @ ~2.0.7
+	https://github.com/me-no-dev/ESPAsyncWebServer.git
+	bblanchon/ArduinoJson@^6.21.2
+	thomasfredericks/Bounce2@ ^2.7.0
 	teckel12/NewPing@^1.9.7
-	rfetick/MPU6050_light@^1.1.0
-	adafruit/Adafruit PWM Servo Driver Library@^2.4.1
 	adafruit/Adafruit SSD1306@^2.5.7
 	adafruit/Adafruit GFX Library@^1.11.5
+	adafruit/Adafruit BusIO@^1.9.3
+	adafruit/Adafruit PWM Servo Driver Library@^2.4.1
+	adafruit/Adafruit ADS1X15@^2.4.0
+	adafruit/Adafruit HMC5883 Unified@^1.2.1
+	adafruit/Adafruit Unified Sensor@^1.1.11
+	plageoj/UrlEncode@ ^1.0.1
+	rfetick/MPU6050_light@^1.1.0
+board_build.partitions = config/no_oat.csv

-board_build.partitions = config/biggest_spiffs.csv
+[env:esp32cam]
+extends = base
+board = esp32cam
@@ -0,0 +1,281 @@
+
+<h1 align="center">
+  <br>
+  <a href="http://www.amitmerchant.com/electron-markdownify">
+  <img src="https://raw.githubusercontent.com/runeharlyk/SpotMicro-Leika/main/assets/logo.jpg" alt="Markdownify" width="200"></a>
+  <br>
+  Spot Micro - Leika
+  <br>
+</h1>
+
+<h4 align="center">A small quadruped robot, inspired by boston dynamic <a href="https://bostondynamics.com/products/spot/" target="_blank">Spot</a>.</h4>
+
+<!-- <p align="center">
+  <a href="https://badge.fury.io/js/electron-markdownify">
+    <img src="https://badge.fury.io/js/electron-markdownify.svg"
+         alt="Gitter">
+  </a>
+  <a href="https://gitter.im/amitmerchant1990/electron-markdownify"><img src="https://badges.gitter.im/amitmerchant1990/electron-markdownify.svg"></a>
+  <a href="https://saythanks.io/to/bullredeyes@gmail.com">
+      <img src="https://img.shields.io/badge/SayThanks.io-%E2%98%BC-1EAEDB.svg">
+  </a>
+  <a href="https://www.paypal.me/AmitMerchant">
+    <img src="https://img.shields.io/badge/$-donate-ff69b4.svg?maxAge=2592000&amp;style=flat">
+  </a>
+</p> -->
+
+<p align="center">
+  <a href="#key-features">Key Features</a> •
+  <a href="#overview">Overview</a> •
+  <a href="#getting-started">Getting started</a> •
+  <a href="#credits">Credits</a> •
+  <a href="#external-links-and-references">Related</a> •
+  <a href="#license">License</a>
+</p>
+
+<!-- GIF
+![screenshot](https://raw.githubusercontent.com/runeharlyk/SpotMicro-Leika/main/assets/logo.jpg) -->
+
+## Key Features
+
+* Live preview - Make changes, See changes
+  * Instantly test and preview code in emulation 
+* Live stream
+  * Camera livestream, battery voltage, servo position, distance sensors and much more.
+* Full kinematic model
+* Dual joystick controller
+* Dark/Light mode
+* Servo calibration tool
+* Full screen mode
+  * Immersive, distraction free.
+* Self hosted, self included
+
+## Overview
+
+This repository contains the complete source code for a Spot Micro quadruped robot.
+Execution of the software takes place on a ESP32 cam, which runs various number of FreeRTos task for seamless robotic operations.
+By focusing on practicality and simplicity in both hardware and software, it offer an accessible platform for learning, experimentation, and modest real-world applications.
+
+The repo is based the following template: [ESP32-rapid-development-template](https://github.com/runeharlyk/ESP32-rapid-development-template)
+
+### Electronics
+
+* ESP32 cam - Brain
+* OV2640 160° - Camera
+* PCA9685 - Servo board
+* 12x 20kg(or higher) servo motors
+* MPU6050 - Inertial measurement unit
+* GY-271 - Magnetometer
+* SZBK07 - 20A DC-DC Buck Converter
+* LM2596 or XL4015 - DC-DC Stepdown Module
+* 2x HC-SR04 - Ultrasonic Distance Sensor
+* 0.96" SD1306 - OLED diplay
+* ACS712 - Current sensor
+* ADS1115 - 16 bit analog to digital converter
+* Power button w/ led
+* 4x 18650 Li-ion battery in 2P2S configuration
+* Couple of resistors (10K, 47.7k, 33K)
+* 4x Servo extension cables
+
+### Body
+
+The robots is 3D printed and is a combination of different Spot Micro designs, with some minor modification on top.
+The original design is developed by KDY0523.
+
+* [robjk reinforced shoulder remix](https://www.thingiverse.com/thing:4937631)
+* [Kooba SpotMicroESP32 remix](https://www.thingiverse.com/thing:4559827)
+* [KDY0532 original design](https://www.thingiverse.com/thing:3445283)
+
+The 3D prints is assembled with some additional component:
+
+* 84x M2x8 screws + M2 nuts
+* 92x M3x8 screws + M3 nuts
+* 64x M3x20 screws + M3 nuts
+* 12x 625ZZ ball bearings
+
+### Software
+
+The software make use of a range of different libraries to enhance the functionality.
+Up to date list can be seen in platformio.ini file.
+The libraries includes:
+
+* AsyncWebServer
+* ArduinoJson
+* Adafruit SSD1306
+* Adafruit GFX Library
+* Adafruit BusIO
+* Adafruit PWM Servo Driver Library
+* Adafruit ADS1X15
+* Adafruit HMC5883 Unified
+* Adafruit Unified Sensor
+* UrlEncode
+* MPU6050 light
+
+#### Structure
+
+The software utilizes a couple of FreeRTos task
+| Task | Description | Priority | Core
+| --- | --- | --- | --- |
+| Idle0 task | Burns cpu time to track cpu usage for core 0 | 1 | 0 |
+| Idle1 task | Burns cpu time to track cpu usage for core 1 | 1 | 1 |
+| JSON writer task | Write serialized JSON objects to SPIFFS at request, with some delay | 2 | 0 |
+| Movement task | Handles movement, updating gait and servos | 3 | 0 |
+| Network task | Handles connection either as STA or AP | 5 | 1 |
+
+#### Feature flags
+
+To dis-/enable the major feature I use defines. Define them in either featureflags.h or in platformio.ini's build_flags.
+| Feature | Description | Default |
+| --- | --- | --- |
+| USE_WIFI | Whether or not to use wifi | 1 |
+| WAIT_FOR_WIFI | Whether or not the device should wait for a connection or restart | 0 |
+| USE_WEBSERVER | Whether or not to use async webserver | 1 |
+| USE_MDNS | Whether or not to use MDNS | 1 |
+| USE_DNS_SERVER | Whether or not to use DNS server | 0 |
+| USE_MPU | Whether or not to use MPU | 1 |
+| USE_POWER_BUTTON | Whether or not to use power button | 1 |
+| USE_USS | Whether or not to use ultra sonic sensors | 1 |
+
+### 📲Web application
+
+The web application is a simple Svelte app, which main focus is to calibrate and control the robot.
+<!-- Write about the emulation, stream, controls and link to the space issues -->
+It is made to be included and hosted by the robot.
+Therefore there is placed a lot of thought behind the functionality and dependencies.
+
+#### Development dependencies
+
+For the development dependencies I choose the following
+
+| Dependencies | Description |
+| --- | --- |
+| Svelte | Svelte is a compiled JS framework that has a very small footprint. Furthermore it make the development process fast and enjoyable. | --- |
+| Vite | Vite is a frontend tool that is used for building fast and optimized web applications. Is serves code local during development and bundles assets for production | --- |
+| Typescript | TypeScript's integration of static typing enhances code reliability and maintainability. | --- |
+| Tailwind CSS | Tailwind CSS accelerates web development with its utility-first approach, ensuring rapid styling and consistent design. | --- |
+| Vite single file plugin | This plugin inline all JS and CSS in the *index.html* file, which is necessary as SPIFFS max file length is 32 bytes. | --- |
+| Vite compression plugin | This plugin compresses the *index.html* file making the final footprint a lot smaller. | --- |
+
+#### Libraries
+
+For the app functionality I choose the following:
+
+| Dependencies | Description |
+| --- | --- |
+| [Three](https://www.npmjs.com/package/three) | Easy to use, lightweight, cross-browser, general purpose 3D library. |
+| [Urdf-loader](https://www.npmjs.com/package/urdf-loader) | Utilities for loading URDF files into THREE.js and a Web Component that loads and renders the model. |
+| [Xacro-parser](https://www.npmjs.com/package/xacro-parser) | Javascript parser and loader for processing the ROS Xacro file format. |
+| [NippleJS](https://www.npmjs.com/package/nipplejs) | A vanilla virtual joystick for touch capable interfaces. |
+| [Uzip](https://www.npmjs.com/package/uzip) | Simple, tiny and fast ZIP library. |
+
+### Space issues
+
+As the ESP32 has very limited storage for program and filesystem, it quickly became a challenge to fit the web app and program in the 4mb of flash.
+Just the robot models files were ≈8mb. So the first step was to minify the files.
+
+My first idea was just to zip the files and then unpack and store them on the browser side.
+This reduced the models footprint to ≈2mb. Great start, but still way to big to fit in flash.
+
+Next step was to make the actual model files smaller, by decimating them.
+Exterior model files where kept in higher resolution while the interior could be reduced more.
+This was the biggest change and made the final footprint of the models ≈550kb.
+A total reduction of ≈94% for the model files.
+
+But the problem was not solved yet. The final *index.html* file was ≈650kb.
+The obvious solution was again to compress it, this time by using gzip.
+This made the file size for the *index.html* 184kb. With all the optimization the data folder was only 756 KB. Awesome!
+
+The last step to fitting the web app on the ESP was to expand SPIFFS part of filesystem image.
+At this step I had to cut some corners to make it fit, mainly by disabling over-the-air update. But a last the filesystem image build and uploaded to the ESP!
+
+## Kinematics
+
+The kinematic for the robot is from this [kinematics paper](https://www.researchgate.net/publication/320307716_Inverse_Kinematic_Analysis_Of_A_Quadruped_Robot)
+
+## Getting started
+
+1. Clone and open the new project
+
+    ```sh
+    git clone https://github.com/runeharlyk/SpotMicroESP32-Leika
+    ```
+
+1. Install dependencies
+
+    ```sh
+    cd app
+    npm install
+    ```
+
+## Usage
+
+### Developing
+
+1. Run the app
+
+    ```sh
+    npm run dev
+    ```
+
+### Building
+
+1. Build the app
+
+    ```sh
+    npm run build
+    ```
+
+1. Upload Filesystem Image using platformIO
+
+## Future
+
+* [ ] Error handling. If something fails flag it, so everything knows is not working.
+* [ ] Host AP with captive portal if initial wifi connection fails or power button is held.
+* [ ] Websocket statistics
+* [ ] Websocket auto reconnect
+* [ ] Iterate to position
+* [ ] Walking gait
+* [ ] Different scene environments (Playground, forest, dessert etc)
+* [ ] Group gridhelper, ground and environment items so they can be moved together
+* [ ] Clamp, lerping of servo motors to a max servo speed (deg/s)
+* [ ] Gait selection (w/ auto option based on speed)
+* [ ] Interactive notebook for learning and playing with the kinematic equations, with extra visuals (full grid system)
+* [ ] Disable servo command
+* [ ] Show arrow forces on robot (accelerometer and center of gravity)
+* [ ] Plane polygon for contact points
+* [ ] Semi transparent boxes for leg workspace
+* [ ] Options button
+
+See the [open issues](https://github.com/runeharlyk/SpotMicroESP32-Leika/issues) for a full list of proposed features (and known issues).
+
+<!-- ## External Links and References -->
+
+## Credits
+
+This project takes great inspiration from the following resources:
+
+1. [Kinematics](https://www.researchgate.net/publication/320307716_Inverse_Kinematic_Analysis_Of_A_Quadruped_Robot)
+1. [Spot Micro Quadruped Project - mike4192](https://github.com/mike4192/spotMicro)
+1. [SpotMicroAi](https://gitlab.com/public-open-source/spotmicroai)
+1. [Spot Micro - Leika](https://github.com/runeharlyk/SpotMicro-Leika/tree/main)
+1. [NightDriverStrip](https://github.com/PlummersSoftwareLLC/NightDriverStrip)
+1. [ESP32-rapid-development-template](https://github.com/runeharlyk/ESP32-rapid-development-template)
+
+## Support
+
+<a href="https://bmc.link/runeharlyk" target="_blank"><img src="https://www.buymeacoffee.com/assets/img/custom_images/purple_img.png" alt="Buy Me A Coffee" style="height: 41px !important;width: 174px !important;box-shadow: 0px 3px 2px 0px rgba(190, 190, 190, 0.5) !important;-webkit-box-shadow: 0px 3px 2px 0px rgba(190, 190, 190, 0.5) !important;" ></a>
+
+## You may also like...
+
+* [Spot Micro Quadruped Project - mike4192](https://github.com/mike4192/spotMicro) - Great ROS based project
+* [SpotMicroAi](https://gitlab.com/public-open-source/spotmicroai) - Group repository with simulations and runtimes
+
+## License
+
+[MIT](https://github.com/runeharlyk/SpotMicroESP32-Leika/blob/master/LICENSE.md)
+
+---
+
+> [runeharlyk.dk](https://runeharlyk.dk) &nbsp;&middot;&nbsp;
+> GitHub [@runeharlyk](https://github.com/runeharlyk) &nbsp;&middot;&nbsp;
+> LinkedIn [@Rune Harlyk](https://www.linkedin.com/in/rune-harlyk/)
@@ -1,4 +1,4 @@
-#include <AsyncJpegStreamHandler.h>
+#include <AsyncJpegStream.h>

 static const char* STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
 static const char* STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
@@ -124,6 +124,5 @@ void streamJpg(AsyncWebServerRequest *request){
        request->send(501);
        return;
    }
-    response->addHeader("Access-Control-Allow-Origin", "*");
    request->send(response);
 }
@@ -0,0 +1,43 @@
+#include <servo.h>
+#include <camera_pins.h>
+#include <esp_camera.h>
+
+esp_err_t InitializeCamera(){
+    camera_config_t camera_config;
+    camera_config.ledc_channel = LEDC_CHANNEL_0;
+    camera_config.ledc_timer = LEDC_TIMER_0;
+    camera_config.pin_d0 = Y2_GPIO_NUM;
+    camera_config.pin_d1 = Y3_GPIO_NUM;
+    camera_config.pin_d2 = Y4_GPIO_NUM;
+    camera_config.pin_d3 = Y5_GPIO_NUM;
+    camera_config.pin_d4 = Y6_GPIO_NUM;
+    camera_config.pin_d5 = Y7_GPIO_NUM;
+    camera_config.pin_d6 = Y8_GPIO_NUM;
+    camera_config.pin_d7 = Y9_GPIO_NUM;
+    camera_config.pin_xclk = XCLK_GPIO_NUM;
+    camera_config.pin_pclk = PCLK_GPIO_NUM;
+    camera_config.pin_vsync = VSYNC_GPIO_NUM;
+    camera_config.pin_href = HREF_GPIO_NUM;
+    camera_config.pin_sscb_sda = SIOD_GPIO_NUM;
+    camera_config.pin_sscb_scl = SIOC_GPIO_NUM;
+    camera_config.pin_pwdn = PWDN_GPIO_NUM;
+    camera_config.pin_reset = RESET_GPIO_NUM;
+    camera_config.xclk_freq_hz = 20000000;
+    camera_config.pixel_format = PIXFORMAT_JPEG; 
+    
+    if(psramFound()){
+        camera_config.frame_size = FRAMESIZE_SVGA;
+        camera_config.jpeg_quality = 10;
+        camera_config.fb_count = 2;
+    } else {
+        camera_config.frame_size = FRAMESIZE_SVGA;
+        camera_config.jpeg_quality = 12;
+        camera_config.fb_count = 1;
+    }
+
+    log_i("Initializing camera");
+    esp_err_t err = esp_camera_init(&camera_config);
+    if (err != ESP_OK) log_e("Camera probe failed with error 0x%x", err);
+
+    return err;
+}
@@ -0,0 +1,55 @@
+#include <HTTPClient.h>
+#include <UrlEncode.h>
+#include <globals.h>
+#include <deviceconfig.h>
+#include <secrets.h>
+
+DRAM_ATTR std::unique_ptr<DeviceConfig> g_ptrDeviceConfig;
+
+DRAM_ATTR size_t g_DeviceConfigJSONBufferSize = 0;
+
+void DeviceConfig::SaveToJSON()
+{
+    g_ptrJSONWriter->FlagWriter(writerIndex);
+}
+
+DeviceConfig::DeviceConfig()
+{
+    // Add SettingSpec for additional settings to this list
+    settingSpecs.emplace_back(
+        NAME_OF(ntpServer),
+        "NTP server address",
+        "The hostname or IP address of the NTP server to be used for time synchronization.",
+        SettingSpec::SettingType::String
+    );
+    settingSpecs.emplace_back(
+        NAME_OF(useMetric),
+        "Use metric system",
+        "A boolean that indicates if unit should be shown in metric ('true'/1) or imperial ('false'/0) format.",
+        SettingSpec::SettingType::Boolean
+    );
+
+    log_i("about to write");
+    writerIndex = g_ptrJSONWriter->RegisterWriter(
+        [this]() { SaveToJSONFile(DEVICE_CONFIG_FILE, g_DeviceConfigJSONBufferSize, *this); }
+    );
+
+    std::unique_ptr<AllocatedJsonDocument> pJsonDoc(nullptr);
+
+    if (LoadJSONFile(DEVICE_CONFIG_FILE, g_DeviceConfigJSONBufferSize, pJsonDoc))
+    {
+        log_i("Loading DeviceConfig from JSON");
+
+        DeserializeFromJSON(pJsonDoc->as<JsonObjectConst>(), true);
+    }
+    else
+    {
+        log_w("DeviceConfig could not be loaded from JSON, using defaults");
+
+        // Set default for additional settings in this code
+        ntpServer = DEFAULT_NTP_SERVER;
+
+        SaveToJSON();
+    }
+}
+
@@ -1,139 +0,0 @@
-#include <ik_task.h>
-
-static const char* TAG = "IK TASK";
-
-position_t spot_position = {.omega=0,.phi=0,.psi=0,.xm=-40,.ym=-170, .zm=0, .set=1};
-position_t goal_position = {0,};
-int16_t servo_angles[4][3] = {{90, 150, 0}, {90, 30, 180}, {90, 150, 0}, {90, 30, 180}};
-int16_t servo_angles_goal[4][3] = {0,};
-
-void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm) {
-  goal_position.omega = omega;
-  goal_position.phi = phi;
-  goal_position.psi = psi;
-  goal_position.xm  = xm;
-  goal_position.ym = ym;
-  goal_position.zm = zm;
-  goal_position.set = true;
-}
-
-void reset_position() {
-    set_orientation_cb(0, 0, 0, 0, 0, 0);
-}
-
-void set_leg_servos() {
-    for (int l = 0; l<4; l++) {
-    // for (int l = 1; l<2; l++) {    
-        for (int s=0;s<3;s++) {
-            if (servo_angles[l][s] != servo_angles_goal[l][s]) {
-                servo_angles[l][s] = servo_angles_goal[l][s];
-                set_servo(l*3 + s, servo_angles[l][s]);
-            }
-        }
-    }
-}
-
-void iterate_to_position() {
-    ESP_LOGI(TAG, "GOAL (%f,%f,%f - %f,%f,%f)", goal_position.omega, goal_position.phi, goal_position.psi, goal_position.xm, goal_position.ym, goal_position.zm);
-
-    do {
-    spot_position.set = false;
-    int diff = 0;
-
-    if (goal_position.omega != spot_position.omega) {
-        diff = goal_position.omega - spot_position.omega;
-        if (abs(diff) < MOTION_STEP_ANGLE) {
-            spot_position.omega = goal_position.omega ;
-        } else {
-            diff = diff < 0 ?  -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
-            spot_position.omega += diff;
-            spot_position.set = true;
-        }
-    }
-
-    if (goal_position.phi != spot_position.phi) {
-        diff = goal_position.phi - spot_position.phi;
-        if (abs(diff) < MOTION_STEP_ANGLE) {
-            spot_position.phi = goal_position.phi ;
-        } else {
-            diff = diff < 0 ?  -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
-            spot_position.phi += diff;
-            spot_position.set = true;
-        }
-    }
-
-    if (goal_position.psi != spot_position.psi) {
-        diff = goal_position.psi - spot_position.psi;
-        if (abs(diff) < MOTION_STEP_ANGLE) {
-            spot_position.psi = goal_position.psi ;
-        } else {
-            diff = diff < 0 ?  -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
-            spot_position.psi += diff;
-            spot_position.set = true;
-        }
-    }
-
-    if (goal_position.xm != spot_position.xm) {
-        diff = goal_position.xm - spot_position.xm;
-        if (abs(diff) < MOTION_STEP_MOVEMENT) {
-            spot_position.xm = goal_position.xm ;
-        } else {
-            diff = diff < 0 ?  -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
-            spot_position.xm += diff;
-            spot_position.set = true;
-        }
-    }
-
-    if (goal_position.ym != spot_position.ym) {
-        diff = goal_position.ym - spot_position.ym;
-        if (abs(diff) < MOTION_STEP_MOVEMENT) {
-            spot_position.ym = goal_position.ym ;
-        } else {
-            diff = diff < 0 ?  -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
-            spot_position.ym += diff;
-            spot_position.set = true;
-        }
-    }
-
-    if (goal_position.zm != spot_position.zm) {
-        diff = goal_position.zm - spot_position.zm;
-        if (abs(diff) < MOTION_STEP_MOVEMENT) {
-            spot_position.zm = goal_position.zm ;
-        } else {
-            diff = diff < 0 ?  -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
-            spot_position.zm += diff;
-            spot_position.set = true;
-        }
-    }
-
-
-    ESP_LOGI(TAG, "CURRENT (%f,%f,%f - %f,%f,%f) %d", spot_position.omega, spot_position.phi, spot_position.psi, spot_position.xm, spot_position.ym, spot_position.zm, spot_position.set);
-    
-    esp_err_t ret = spot_IK(spot_position.omega*DEGREES2RAD, spot_position.phi*DEGREES2RAD, spot_position.psi*DEGREES2RAD, spot_position.xm, spot_position.ym, spot_position.zm, servo_angles_goal);
-    ESP_LOGD(TAG, "Valid IK %d", ret==ESP_OK);
-    if (ret == ESP_OK) {
-        //print_int_matrix((int16_t*) servo_angles_goal, 4, 3, "servo_angles_goal", false);
-        set_leg_servos();
-    }
-
-    } while (spot_position.set);
-
-
-    //set_new_orientation_act_value((int16_t) spot_position.omega, (int16_t) spot_position.phi, (int16_t) spot_position.psi, (int16_t) spot_position.xm, (int16_t) spot_position.ym, (int16_t) spot_position.zm);
-}
-
-void task_ik(void *ignore){
-    ESP_LOGI(TAG, "Executing on core %d", xPortGetCoreID());
-    esp_err_t ret;
-
-    reset_position();
-    
-    for(;;) {
-        vTaskDelay(100 / portTICK_RATE_MS);
-        if (goal_position.set) {
-            goal_position.set = false;
-            iterate_to_position();
-        }
-    }
-    vTaskDelete(NULL);
-}
@@ -0,0 +1,177 @@
+#include <globals.h>
+#include <SPIFFS.h>
+#include <jsonserializer.h>
+#include <taskmanager.h>
+
+DRAM_ATTR std::unique_ptr<JSONWriter> g_ptrJSONWriter = nullptr;
+
+bool LoadJSONFile(const char *fileName, size_t& bufferSize, std::unique_ptr<AllocatedJsonDocument>& pJsonDoc)
+{
+    bool jsonReadSuccessful = false;
+
+    File file = SPIFFS.open(fileName);
+
+    if (file)
+    {
+        if (file.size() > 0)
+        {
+            log_i("Attempting to read JSON file %s", fileName);
+
+            if (bufferSize == 0)
+                bufferSize = std::max((size_t)JSON_BUFFER_BASE_SIZE, file.size());
+
+            // Loop is here to deal with out of memory conditions
+            while(true)
+            {
+                pJsonDoc.reset(new AllocatedJsonDocument(bufferSize));
+
+                DeserializationError error = deserializeJson(*pJsonDoc, file);
+
+                if (error == DeserializationError::NoMemory)
+                {
+                    pJsonDoc.reset(nullptr);
+                    file.seek(0);
+                    bufferSize += JSON_BUFFER_INCREMENT;
+
+                    log_w("Out of memory reading JSON from file %s - increasing buffer to %zu bytes", fileName, bufferSize);
+                }
+                else if (error == DeserializationError::Ok)
+                {
+                    jsonReadSuccessful = true;
+                    break;
+                }
+                else
+                {
+                    log_w("Error with code %d occurred while deserializing JSON from file %s", to_value(error.code()), fileName);
+                    break;
+                }
+            }
+        }
+
+        file.close();
+    }
+
+    return jsonReadSuccessful;
+}
+
+void SerializeWithBufferSize(std::unique_ptr<AllocatedJsonDocument>& pJsonDoc, size_t& bufferSize, std::function<bool(JsonObject&)> serializationFunction)
+{
+    // Loop is here to deal with out of memory conditions
+    while(true)
+    {
+        pJsonDoc.reset(new AllocatedJsonDocument(bufferSize));
+        JsonObject jsonObject = pJsonDoc->to<JsonObject>();
+
+        if (serializationFunction(jsonObject))
+            break;
+
+        pJsonDoc.reset(nullptr);
+        bufferSize += JSON_BUFFER_INCREMENT;
+
+        log_w("Out of memory serializing object - increasing buffer to %zu bytes", bufferSize);
+    }
+}
+
+bool SaveToJSONFile(const char *fileName, size_t& bufferSize, IJSONSerializable& object)
+{
+    if (bufferSize == 0)
+        bufferSize = JSON_BUFFER_BASE_SIZE;
+
+    std::unique_ptr<AllocatedJsonDocument> pJsonDoc(nullptr);
+
+    SerializeWithBufferSize(pJsonDoc, bufferSize, [&object](JsonObject& jsonObject) { return object.SerializeToJSON(jsonObject); });
+
+    SPIFFS.remove(fileName);
+
+    File file = SPIFFS.open(fileName, FILE_WRITE);
+
+    if (!file)
+    {
+        log_e("Unable to open file %s to write JSON!", fileName);
+        return false;
+    }
+
+    size_t bytesWritten = serializeJson(*pJsonDoc, file);
+    log_i("Number of bytes written to JSON file %s: %d", fileName, bytesWritten);
+
+    file.flush();
+    file.close();
+
+    if (bytesWritten == 0)
+    {
+        log_e("Unable to write JSON to file %s!", fileName);
+        SPIFFS.remove(fileName);
+        return false;
+    }
+
+/*
+    file = SPIFFS.open(fileName);
+    if (file)
+    {
+        while (file.available())
+            Serial.write(file.read());
+
+        file.close();
+    }
+*/
+
+    return true;
+}
+
+bool RemoveJSONFile(const char *fileName)
+{
+    return SPIFFS.remove(fileName);
+}
+
+size_t JSONWriter::RegisterWriter(std::function<void()> writer)
+{
+    // Add the writer with its flag unset
+    writers.emplace_back(writer);
+    return writers.size() - 1;
+}
+
+void JSONWriter::FlagWriter(size_t index)
+{
+    // Check if we received a valid writer index
+    if (index >= writers.size())
+        return;
+
+    writers[index].flag = true;
+    latestFlagMs = millis();
+
+    g_TaskManager.NotifyJSONWriterThread();
+}
+
+// JSONWriterTaskEntry
+//
+// Invoke functions that write serialized JSON objects to SPIFFS at request, with some delay
+void IRAM_ATTR JSONWriterTaskEntry(void *)
+{
+    for(;;)
+    {
+        TickType_t notifyWait = portMAX_DELAY;
+
+        for (;;)
+        {
+            // Wait until we're woken up by a writer being flagged, or until we've reached the hold point
+            ulTaskNotifyTake(pdTRUE, notifyWait);
+
+            if (!g_ptrJSONWriter)
+                continue;
+
+            unsigned long holdUntil = g_ptrJSONWriter->latestFlagMs + JSON_WRITER_DELAY;
+            unsigned long now = millis();
+            if (now >= holdUntil)
+                break;
+
+            notifyWait = pdMS_TO_TICKS(holdUntil - now);
+        }
+
+        for (auto &entry : g_ptrJSONWriter->writers)
+        {
+            // Unset flag before we do the actual write. This makes that we don't miss another flag raise if it happens while writing
+            if (entry.flag.exchange(false))
+                entry.writer();
+        }
+    }
+}
@@ -1,11 +1,32 @@
-#include <spot.h>
+#include <globals.h>
+#include <deviceconfig.h>

-Spot spot;

-void setup(){
-  spot.boot();
+ESPTaskManager g_TaskManager;
+#if USE_WIFI && USE_WEBSERVER
+    DRAM_ATTR CWebServer g_WebServer;
+#endif
+
+void setup() {
+    WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
+    Serial.begin(BAUDRATE);
+    log_i("Booting");
+    SPIFFS.begin();
+    Wire.begin(SDA, SCL);
+    InitializeCamera();
+    g_TaskManager.begin();
+    g_TaskManager.StartThreads();
+
+    g_ptrJSONWriter = std::make_unique<JSONWriter>();
+    g_ptrDeviceConfig = std::make_unique<DeviceConfig>();
+    g_ptrServo = std::make_unique<Servo>();
+
+    g_ptrServo->begin();
+    g_ptrServo->setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
+    g_ptrServo->setPWMFreq(SERVO_FREQ);
+    g_ptrServo->updateServos();
 }

-void loop(){
-  spot.handle();
+void loop() {
+    delay(200);
 }
@@ -0,0 +1,79 @@
+#include <featureflags.h>
+#if USE_MPU
+    #include <MPU6050_light.h>
+    #include <Adafruit_Sensor.h>
+    #include <Adafruit_HMC5883_U.h>
+#endif
+
+#if USE_POWER_BUTTON
+    #include <Bounce2.h>
+#endif
+
+#if USE_POWER_BUTTON
+    Bounce2::Button PowerButton;
+#endif
+
+#if USE_MPU
+    DRAM_ATTR MPU6050 g_imu(Wire);
+    DRAM_ATTR Adafruit_HMC5883_Unified g_mag = Adafruit_HMC5883_Unified(12345);
+#endif
+
+void beginMag() {
+    if(!g_mag.begin()) log_w("Failed to initialize HMC5883L");
+}
+
+void beginImu() {
+    byte status = g_imu.begin();
+    if(status != 0) log_w("MPU initialize failed");
+    delay(100);
+    g_imu.calcOffsets(true,true);
+}
+
+float getHeading() {
+    sensors_event_t event;
+    g_mag.getEvent(&event);
+
+    float heading = atan2(event.magnetic.y, event.magnetic.x);
+    float declinationAngle = 0.22;
+    heading += declinationAngle;
+    if(heading < 0) heading += 2*PI;
+    if(heading > 2*PI) heading -= 2*PI;
+    return heading * 180/M_PI;
+}
+
+float getTemp() {
+    return g_imu.getTemp();
+}
+
+float getAngleX() {
+    return g_imu.getAngleX();
+}
+
+float getAngleY() {
+    return g_imu.getAngleX();
+}
+
+float getAngleZ() {
+    return g_imu.getAngleZ();
+}
+
+void IRAM_ATTR MovementHandlingLoopEntry(void *) {
+    TickType_t notifyWait = 0;
+    beginMag();
+    beginImu();
+
+    for (;;) {
+        g_imu.update();
+        ulTaskNotifyTake(pdTRUE, notifyWait);
+
+        // #if USE_POWER_BUTTON
+        //     PowerButton.update();
+        //     if (PowerButton.pressed()) {
+        //         log_i("Power Button Pressed");
+        //         g_ptrServo->toggleState();
+        //     }
+        // #endif
+
+        notifyWait = pdMS_TO_TICKS(100);
+    }
+}
@@ -0,0 +1,96 @@
+#include <globals.h>
+#include <deviceconfig.h>
+
+#if USE_WIFI && USE_WEBSERVER
+    extern DRAM_ATTR CWebServer g_WebServer;
+#endif
+
+#if USE_WIFI
+
+bool ConnectToWiFi(uint cRetries) {
+    static bool bPreviousConnection = false;
+
+    if (WiFi.isConnected()) return true;
+
+    log_i("Connection to wifi");
+    WiFi.disconnect();
+    WiFi.mode(WIFI_STA);
+    WiFi.setHostname(HOSTNAME);
+
+    for (uint iPass = 0; iPass < cRetries; iPass++) {
+        log_i("Pass %u of %u: Connecting to Wifi SSID: \"%s\" - ESP32 Free Memory: %u, PSRAM:%u, PSRAM Free: %u\n",
+                iPass + 1, cRetries, SSID, ESP.getFreeHeap(), ESP.getPsramSize(), ESP.getFreePsram());
+
+        WiFi.begin(SSID, PASS);
+        delay(4000 + iPass * 1000);
+
+        if (WiFi.isConnected()) {
+            log_i("Connected to AP with BSSID: %s\n", WiFi.BSSIDstr().c_str());
+            break;
+        }
+    }
+
+    // Additional Services onwwards reliant on network so close if not up.
+    if (false == WiFi.isConnected()) {
+        log_i("Giving up on WiFi\n");
+        return false;
+    }
+
+    log_i("Received IP: %s", WiFi.localIP().toString().c_str());
+
+    // If we were connected before, network-dependent services will have been started already
+    if (bPreviousConnection)
+        return true;
+
+    #if USE_OTA
+        //debugI("Publishing OTA...");
+        SetupOTA(String(cszHostname));
+    #endif
+
+    #if USE_NTP
+        //debugI("Setting Clock...");
+        //NTPTimeClient::UpdateClockFromWeb(&g_Udp);
+    #endif
+
+    #if USE_WEBSERVER
+        g_WebServer.begin();
+    #endif
+    
+    #if USE_MDNS
+    if(MDNS.begin(HOSTNAME)){
+        MDNS.addService("http", "tcp", HTTP_PORT);
+    }
+    #endif
+
+    bPreviousConnection = true;
+    return true;
+}
+
+void IRAM_ATTR NetworkHandlingLoopEntry(void *) {
+    unsigned long lastWifiCheck = 0;
+    unsigned long checkWiFiEveryMs = 1000;
+
+    TickType_t notifyWait = 0;
+
+    for (;;) {
+        ulTaskNotifyTake(pdTRUE, notifyWait);
+
+        if ( millis() - lastWifiCheck > checkWiFiEveryMs) {
+            if (WiFi.isConnected() == false && ConnectToWiFi(5) == false) {
+                log_e("Cannot Connect to Wifi!");
+                #if WAIT_FOR_WIFI
+                    log_e("Rebooting in 5 seconds due to no Wifi available.");
+                    delay(5000);
+                    throw new std::runtime_error("Rebooting due to no Wifi available.");
+                #endif
+            }
+        }
+        #if USE_WEBSERVER
+        g_WebServer.loop();
+        #endif
+
+        notifyWait = pdMS_TO_TICKS(1000);
+    }
+}
+
+#endif // ENABLE_WIFI
@@ -1,38 +1,3 @@
-#include <Adafruit_PWMServoDriver.h>
+#include <servo.h>

-#include "servo.h"
-#include "config.h"
-
-#include "servo_config.h"
-
-static const char* TAG = "SERVO";
-
-Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
-
-const int16_t servo_min[12] = {153,118,138,121,116,125,131,150,148,130,158,165};
-const float servo_conversion[12] = {2.011111,2.011111,2.000000,2.050000,1.966667,2.027778,2.038889,1.677778,1.622222,2.027778,1.927778,1.650000};
-const int8_t servo_invert[12] = {1,0,1, 0,1,0,  0,0,1,  1,1,0};
-const float theta_range[3][2] = {{-M_PI / 3, M_PI/3}, {-2 * M_PI/3, M_PI/3}, {0, M_PI}};
-
-esp_err_t disable_servos(){
-  ESP_LOGI(TAG, "Disabling servos");
-  pwm.setPWM(0, 0, 0);
-  return ESP_OK;
-}
-
-esp_err_t setup_pwm_controller(){
-  pwm.begin();
-  pwm.setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
-  pwm.setPWMFreq(SERVO_FREQ);
-  return ESP_OK;
-}
-
-esp_err_t set_servo(uint8_t id, uint16_t angle) {
-  esp_err_t ret;
-  uint16_t pulse = (uint16_t) (0.5 + servo_min[id] + (angle * servo_conversion[id]));
-  ESP_LOGI(TAG, "setPWM of servo %d, %d degrees -> Pulse %d", id, angle, pulse);
-  ret = pwm.setPWM(id, 0, pulse);
-
-  if (ret == ESP_OK) return ESP_OK;
-  else return ESP_FAIL;
-}
+DRAM_ATTR std::unique_ptr<Servo> g_ptrServo;
@@ -1,241 +0,0 @@
-#include <spot.h>
-
-websocket_message wsm;
-
-Spot::Spot() 
-    : _events(EVENTSOURCE_PATH)
-    , _ws(WEBSOCKET_PATH)
-    , _server(HTTP_PORT)
-    , _display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, SCREEN_RESET)
-    , _mpu(Wire)
-    , _leftUss(USS_LEFT, USS_LEFT, USS_MAX_DISTANCE)
-    , _rightUss(USS_RIGHT, USS_RIGHT, USS_MAX_DISTANCE)
-    , _pwm(0x40)
-{ }
-
-esp_err_t Spot::boot(){
-    log_i("Booting...");
-    WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
-    SPIFFS.begin();
-    Wire.begin(SDA, SCL);
-    _initialize_button();
-
-    _initialize_display();
-    _initialize_mpu();
-    _initialize_camera();
-
-    initialize_wifi();
-    _initialize_arduino_oat();
-    _initialize_server();
-    log_i("Done booting");
-    return ESP_OK;
-}
-
-void Spot::handle(){
-    if(USE_CAPTIVE_PORTAL) _dnsServer.processNextRequest();
-    ArduinoOTA.handle();
-    _ws.cleanupClients();
-
-    broadcast_data();
-}
-
-esp_err_t Spot::initialize_wifi(){
-    if(USE_CAPTIVE_PORTAL) _initialize_captive_portal();
-    else _initialize_wifi_connection();
-    if(MDNS.begin(HOSTNAME)){
-        MDNS.addService("http", "tcp", HTTP_PORT);
-    }
-
-    if(USE_CAPTIVE_PORTAL) display_ip_and_ssid(&_display, WiFi.softAPIP().toString(), HOSTNAME);
-    else display_ip_and_ssid(&_display, WiFi.localIP().toString(), SSID);
-    return ESP_OK;
-}
-
-uint8_t Spot::cpu_temperature(){
-    return (temprature_sens_read() - 32) / 1.8;
-}
-
-esp_err_t Spot::broadcast_data(){
-    if(millis() - _last_broadcast < 50) return ESP_OK;
-
-    _mpu.update();
-    size_t numContent = 14;
-    float content[numContent] = {
-        WiFi.RSSI(),
-        _mpu.getTemp(),
-        _mpu.getAngleX(),
-        _mpu.getAngleY(),
-        _mpu.getAngleZ(),
-        cpu_temperature(),
-        _leftUss.ping_cm(),
-        _rightUss.ping_cm(),
-        ESP.getFreeHeap(),
-        ESP.getFreePsram(),
-        ESP.getMinFreeHeap(),
-        ESP.getMinFreePsram(),
-        ESP.getMaxAllocHeap(),
-        ESP.getMaxAllocPsram(),
-    };
-
-    uint8_t* buf = (uint8_t*) &content;
-    size_t buf_len = sizeof(buf);
-
-    _ws.binaryAll(buf, buf_len * numContent);
-    _last_broadcast = millis();
-
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_camera(){
-    camera_config_t camera_config;
-    camera_config.ledc_channel = LEDC_CHANNEL_0;
-    camera_config.ledc_timer = LEDC_TIMER_0;
-    camera_config.pin_d0 = Y2_GPIO_NUM;
-    camera_config.pin_d1 = Y3_GPIO_NUM;
-    camera_config.pin_d2 = Y4_GPIO_NUM;
-    camera_config.pin_d3 = Y5_GPIO_NUM;
-    camera_config.pin_d4 = Y6_GPIO_NUM;
-    camera_config.pin_d5 = Y7_GPIO_NUM;
-    camera_config.pin_d6 = Y8_GPIO_NUM;
-    camera_config.pin_d7 = Y9_GPIO_NUM;
-    camera_config.pin_xclk = XCLK_GPIO_NUM;
-    camera_config.pin_pclk = PCLK_GPIO_NUM;
-    camera_config.pin_vsync = VSYNC_GPIO_NUM;
-    camera_config.pin_href = HREF_GPIO_NUM;
-    camera_config.pin_sscb_sda = SIOD_GPIO_NUM;
-    camera_config.pin_sscb_scl = SIOC_GPIO_NUM;
-    camera_config.pin_pwdn = PWDN_GPIO_NUM;
-    camera_config.pin_reset = RESET_GPIO_NUM;
-    camera_config.xclk_freq_hz = 20000000;
-    camera_config.pixel_format = PIXFORMAT_JPEG; 
-    
-    if(psramFound()){
-        camera_config.frame_size = FRAMESIZE_SVGA;
-        camera_config.jpeg_quality = 10;
-        camera_config.fb_count = 2;
-    } else {
-        camera_config.frame_size = FRAMESIZE_SVGA;
-        camera_config.jpeg_quality = 12;
-        camera_config.fb_count = 1;
-    }
-
-    log_i("Initializing camera");
-    esp_err_t err = esp_camera_init(&camera_config);
-    if (err != ESP_OK) log_i("Camera probe failed with error 0x%x", err);
-
-    return err;
-}
-
-esp_err_t Spot::_initialize_wifi_connection(){
-    log_i("Connecting to wifi");
-    WiFi.begin(SSID, PASS);
-    int8_t timeout = 0;
-    while (WiFi.status() != WL_CONNECTED) {
-        delay(500);
-        timeout++;
-        if(timeout > 15) {
-            _initialize_captive_portal();
-            return ESP_FAIL;
-        }
-    }
-    log_i("Connected successfully");
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_arduino_oat(){
-    log_i("Starting ArduinoOTA");
-    ArduinoOTA.onStart([&]() { _events.send("Update Start", "ota"); });
-    ArduinoOTA.onEnd([&]() { _events.send("Update End", "ota"); });
-    ArduinoOTA.onProgress([&](unsigned int progress, unsigned int total) {
-        char p[32];
-        sprintf(p, "Progress: %u%%\n", (progress/(total/100)));
-        _events.send(p, "ota");
-    });
-    ArduinoOTA.onError([&](ota_error_t error) {
-        if(error == OTA_AUTH_ERROR) _events.send("Auth Failed", "ota");
-        else if(error == OTA_BEGIN_ERROR) _events.send("Begin Failed", "ota");
-        else if(error == OTA_CONNECT_ERROR) _events.send("Connect Failed", "ota");
-        else if(error == OTA_RECEIVE_ERROR) _events.send("Recieve Failed", "ota");
-        else if(error == OTA_END_ERROR) _events.send("End Failed", "ota");
-    });
-    ArduinoOTA.setHostname(HOSTNAME);
-    ArduinoOTA.begin();
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_captive_portal(){
-    log_i("Starting captive portal");
-    WiFi.softAP(HOSTNAME);
-    _dnsServer.start(53, "*", WiFi.softAPIP());
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_server(){
-    log_i("Starting webserver");
-    _ws.onEvent(onWsEvent);
-    _server.addHandler(&_ws);
-    _server.on("/stream", HTTP_GET, streamJpg);
-    _server.serveStatic("/", SPIFFS, "/").setDefaultFile("index.html");
-    _server.begin();
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_display(){
-    log_i("Initializing display");
-    if(!_display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
-        log_w("SSD1306 allocation failed");
-        return ESP_FAIL;
-    }
-
-    _display.display();
-    delay(200);
-    _display.clearDisplay();
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_mpu(){
-    log_i("Initializing MPU");
-    byte status = _mpu.begin();
-    if(status != 0) {
-        log_w("MPU initialize failed");
-        return ESP_FAIL;
-    }
-    log_i("Calculating offsets, do not move MPU6050");
-    delay(1000);
-    log_i("Cone calculating offsets");
-    _mpu.calcOffsets(true,true);
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_pwm_controller(){
-    log_i("Initializing PWM controller");
-    _pwm.begin();
-    _pwm.setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
-    _pwm.setPWMFreq(SERVO_FREQ);
-    return ESP_OK;
-}
-
-esp_err_t Spot::_initialize_button() {
-    pinMode(BUTTON_LED, OUTPUT);
-    digitalWrite(BUTTON_LED, HIGH);
-    //pinMode(BUTTON, INPUT);
-    return ESP_OK;
-}
-
-void display_ip_and_ssid(Adafruit_SSD1306* display, String ip, const char* ssid) {
-    display->setTextColor(WHITE, BLACK);
-    display->setTextSize(1);
-    int16_t x1 = 0;
-    int16_t y1 = 0;
-    uint16_t h = 0;
-    uint16_t w = 0;
-    
-    display->getTextBounds(ssid, 0, 0, &x1, &y1, &w, &h);
-    display->setCursor(SCREEN_WIDTH/2 - w/2, SCREEN_HEIGHT/2-8 - h/2);
-    display->println(ssid);
-
-    display->getTextBounds(ip, 0, 0, &x1, &y1, &w, &h);
-    display->setCursor(SCREEN_WIDTH/2 - w/2, SCREEN_HEIGHT/2+8 - h/2);
-    display->println(ip);
-    display->display();
-}
@@ -1,264 +0,0 @@
-#include "spot_ik.h"
-#include "IK_config.h"
-
-#include "esp_dsp.h"
-#include "math.h"
-
-// https://github.com/maartenweyn/SpotMicro_ESP32/blob/master/code/esp-idf/ik_test/main/spot_ik.c
-
-static const char* TAG = "IK";
-
-#define RAD2DEGREES 57.295779513082321 // 180 / PI
-#define DEGREES2RAD 0.017453292519943
-
-// float orientation[3] = {M_PI / 8, M_PI / 8, 0}; //A 3x1 arry with Spot's Roll, Pitch, Yaw angles - omega, phi, psi
-// float position[3] = {0, 0, 0};    //A 3x1 array with Spot's X, Y, Z coordinates
-static float Rx[4][4] = {{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // X-axis rotation matrix
-static float Ry[4][4] = {{0, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // Y-axis rotation matrix
-static float Rz[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; // Z-axis rotation matrix
-static float Rxyz[4][4] = {0,};
-static float Tm[4][4] = {0,}; // Transformation Matrix
-static float T[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; //Translation <Atrix
-static float Trb[4][4] = {0,}; //Tranformation Matrix Right Back
-static float Trf[4][4] = {0,}; //Tranformation Matrix Right Front
-static float Tlb[4][4] = {0,}; //Tranformation Matrix Left Back
-static float Tlf[4][4] = {0,}; //Tranformation Matrix Left Front
-
-static const float Trb2[4][4] = {{0, 0, 1, -L/2}, {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Back
-static const float Trf2[4][4] = {{0, 0, 1, L/2},  {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Front
-static const float Tlb2[4][4] = {{0, 0, 1, -L/2},  {0, 1, 0, 0}, {-1, 0, 0, W/2},  {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Back
-static const float Tlf2[4][4] = {{0, 0, 1, L/2}, {0, 1, 0, 0}, {-1, 0, 0, W/2},  {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Front
-
-static float Lp[4][4] = {{80, -130, 100, 1}, {80, -130, -100, 1}, {-130, -130, 100, 1}, {-130, -130, -100, 1}};
-
-static const float Ix[4][4] = {{-1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
-
-static esp_err_t inverse(float a[4][4], float b[4][4]) {
-    float s0 = a[0][0] * a[1][1] - a[1][0] * a[0][1];
-    float s1 = a[0][0] * a[1][2] - a[1][0] * a[0][2];
-    float s2 = a[0][0] * a[1][3] - a[1][0] * a[0][3];
-    float s3 = a[0][1] * a[1][2] - a[1][1] * a[0][2];
-    float s4 = a[0][1] * a[1][3] - a[1][1] * a[0][3];
-    float s5 = a[0][2] * a[1][3] - a[1][2] * a[0][3];
-
-    float c5 = a[2][2] * a[3][3] - a[3][2] * a[2][3];
-    float c4 = a[2][1] * a[3][3] - a[3][1] * a[2][3];
-    float c3 = a[2][1] * a[3][2] - a[3][1] * a[2][2];
-    float c2 = a[2][0] * a[3][3] - a[3][0] * a[2][3];
-    float c1 = a[2][0] * a[3][2] - a[3][0] * a[2][2];
-    float c0 = a[2][0] * a[3][1] - a[3][0] * a[2][1];
-
-    // Should check for 0 determinant
-    float det = (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
-
-    if (det == 0.0) return ESP_FAIL;
-
-    float invdet = 1.0 / det;
-
-    b[0][0] = ( a[1][1] * c5 - a[1][2] * c4 + a[1][3] * c3) * invdet;
-    b[0][1] = (-a[0][1] * c5 + a[0][2] * c4 - a[0][3] * c3) * invdet;
-    b[0][2] = ( a[3][1] * s5 - a[3][2] * s4 + a[3][3] * s3) * invdet;
-    b[0][3] = (-a[2][1] * s5 + a[2][2] * s4 - a[2][3] * s3) * invdet;
-
-    b[1][0] = (-a[1][0] * c5 + a[1][2] * c2 - a[1][3] * c1) * invdet;
-    b[1][1] = ( a[0][0] * c5 - a[0][2] * c2 + a[0][3] * c1) * invdet;
-    b[1][2] = (-a[3][0] * s5 + a[3][2] * s2 - a[3][3] * s1) * invdet;
-    b[1][3] = ( a[2][0] * s5 - a[2][2] * s2 + a[2][3] * s1) * invdet;
-
-    b[2][0] = ( a[1][0] * c4 - a[1][1] * c2 + a[1][3] * c0) * invdet;
-    b[2][1] = (-a[0][0] * c4 + a[0][1] * c2 - a[0][3] * c0) * invdet;
-    b[2][2] = ( a[3][0] * s4 - a[3][1] * s2 + a[3][3] * s0) * invdet;
-    b[2][3] = (-a[2][0] * s4 + a[2][1] * s2 - a[2][3] * s0) * invdet;
-
-    b[3][0] = (-a[1][0] * c3 + a[1][1] * c1 - a[1][2] * c0) * invdet;
-    b[3][1] = ( a[0][0] * c3 - a[0][1] * c1 + a[0][2] * c0) * invdet;
-    b[3][2] = (-a[3][0] * s3 + a[3][1] * s1 - a[3][2] * s0) * invdet;
-    b[3][3] = ( a[2][0] * s3 - a[2][1] * s1 + a[2][2] * s0) * invdet;
-
-    return ESP_OK;
-}
-
-void print_matrix(float * matrix, int n, int m, char* name) {
-  printf("Matrix %s:\n", name);
-  for (int i=0;i<n;i++) {
-    for (int j=0;j<m;j++)
-      printf("%.2f ", matrix[i*m + j]);
-    printf("\n");
-  }
-}
-
-void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines) {
-  printf("Matrix %s:\n", name);
-  for (int i=0;i<n;i++) {
-    for (int j=0;j<m;j++)
-      printf("%3d ", matrix[i*m + j]);
-    if (newlines) printf("\n");
-  }
-  if (!newlines) printf("\n");
-}
-
-esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm) {
-  float cos_omega = cos(omega);
-  float sin_omega = sin(omega);
-  float cos_phi   = cos(phi);
-  float sin_phi   = sin(phi);
-  float cos_psi   = cos(psi);
-  float sin_psi   = sin(psi);
-
-  Rx[1][1] = cos_omega;
-  Rx[1][2] = -sin_omega;
-  Rx[2][1] = sin_omega;
-  Rx[2][2] = cos_omega;
-
-  Ry[0][0] = cos_phi;
-  Ry[0][2] = sin_phi;
-  Ry[2][0] = -sin_phi;
-  Ry[2][2] = cos_phi;
-
-  Rz[0][0] = cos_psi;
-  Rz[0][1] = -sin_psi;
-  Rz[1][0] = sin_psi;
-  Rz[1][1] = cos_psi;
-
-  float Rxy[4][4];
-  dspm_mult_f32_ae32((float*) Rx, (float*) Ry, (float*) Rxy, 4, 4, 4);
-  dspm_mult_f32_ae32((float*) Rxy, (float*) Rz, (float*) Rxyz, 4, 4, 4);
-
-  // print_matrix((float*) Rx, 4, 4, "Rx");
-  // print_matrix((float*) Ry, 4, 4, "Ry");
-  // print_matrix((float*) Rz, 4, 4, "Rz");
-  // print_matrix((float*) Rxyz, 4, 4, "Rxyz");
-
-  T[0][3] = xm;
-  T[1][3] = ym;
-  T[2][3] = zm;
-
-  dsps_add_f32_ae32((float*) T, (float*) Rxyz, (float*) Tm, 16, 1, 1, 1);
-  // print_matrix((float*) Tm, 4, 4, "Tm");
-
-  dspm_mult_f32_ae32((float*) Tm, (float*) Trb2, (float*) Trb, 4, 4, 4);
-  dspm_mult_f32_ae32((float*) Tm, (float*) Trf2, (float*) Trf, 4, 4, 4);
-  dspm_mult_f32_ae32((float*) Tm, (float*) Tlf2, (float*) Tlf, 4, 4, 4);
-  dspm_mult_f32_ae32((float*) Tm, (float*) Tlb2, (float*) Tlb, 4, 4, 4);
-  // print_matrix((float*) Trb, 4, 4, "Trb");
-  // print_matrix((float*) Trf, 4, 4, "Trf");
-  // print_matrix((float*) Tlf, 4, 4, "Tlf");
-  // print_matrix((float*) Tlb, 4, 4, "Tlb");
-
-  return ESP_OK;
-}
-
-
-
-esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t* servo_angles) {
-    //float x = (leg_id == LEG_RF || leg_id == LEG_RB) ? -p[0] : p[0];
-    // Rxy Lenght of shoulder-point on x/y plane only
-    float Rxy2 = p[0]*p[0] + p[1]*p[1] - L1L1;
-    if (Rxy2 < 0) Rxy2 = 0;
-    float Rxy = sqrt(Rxy2);
-    // Dxy Distance we need to cover in xy plane
-    float Dxy = Rxy - L2;
-    // Dxyz 3D Distance we need to cover
-    float Dxyz = sqrt(Dxy*Dxy+p[2]*p[2]);
-
-    // ESP_LOGD(tag, "IK Rxy,Dxy,Dxyz %.2f %.2f %.2f", Rxy, Dxy, Dxyz);
-
-    // The angle we need to cover - the angle already covered because of the offset of the servo
-    float theta1 = -atan2(p[1], p[0]) - atan2(Rxy, -L1);
-    if (isnan(theta1)) return -1;
-
-    // ESP_LOGD(tag, "T1 %.2f = (- %.2f - %.2f)", theta1, -atan2(p[1], p[0]), atan2(Rxy, -L1));
-
-    float d=(Dxyz*Dxyz - L3L3 - L4L4)/(LL34);
-    float theta3 = acos(d);
-    if (isnan(theta3)) return -3;
-
-    float theta2 = atan2(p[2], Dxy) - atan2(L4 * sin(theta3), L3 + L4 * cos(theta3));
-    if (isnan(theta2)) return -2;
-    //  ESP_LOGD(tag, "IK %.2f - atan2( %.2f, %.2f) (%.2f)", atan2(p[2], Dxy), L4 * sin(theta3), L3 + L4 * cos(theta3), atan2(L4 * sin(theta3), L3 + L4 * cos(theta3)));
-
-
-    if (servo_invert[leg_id*3]) {
-      servo_angles[0] = 90 - (int16_t) (0.5 + theta1 * RAD2DEGREES);
-    } else {
-      servo_angles[0] = 90 + (int16_t) (0.5 + theta1 * RAD2DEGREES);
-    }
-
-    if (servo_invert[leg_id*3 + 1]) {
-      servo_angles[1] = 120 + (int16_t) (0.5 + theta2 * RAD2DEGREES);
-    } else {
-      servo_angles[1] = 60 - (int16_t) (0.5 + theta2 * RAD2DEGREES);
-    }
-
-    if (servo_invert[leg_id*3 + 2]) {
-      servo_angles[2] = 180 - (int16_t) (0.5 + theta3 * RAD2DEGREES);
-    } else {
-      servo_angles[2] = (int16_t) (0.5 + theta3 * RAD2DEGREES);
-    }
-
-    if (servo_angles[1] < 0) servo_angles[1] += 360;
-    if (servo_angles[1] > 360) servo_angles[1] -= 360;
-
-    // ESP_LOGD(tag, "IK T2 %.2f -> %d", theta2, servo_angles[1]);
-    // ESP_LOGD(tag, "IK D %.2f, T3 %.2f -> %d", d, theta3, servo_angles[2]);
-
-
-    if (theta1 < theta_range[0][0] || theta1 > theta_range[0][1]) return -4;
-    if (theta2 < theta_range[1][0] || theta2 > theta_range[1][1]) return -5;
-    if (theta3 < theta_range[2][0] || theta3 > theta_range[2][1]) return -6;
-
-
-    return ESP_OK;
-}
-
-esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]) {
-  esp_err_t ret = ESP_OK;
-  body_IK(omega, phi, psi, xm, ym, zm);
-
-  float inv[4][4];
-  float Q1[4][4];
-  float Q[4];
-
-
-  ret += inverse(Tlf, inv);
-  dspm_mult_f32_ae32((float*) inv, (float*) Lp[0], (float*) Q, 4, 4, 1);
-  // print_matrix((float*) Q, 1, 4, "Q LF");
-  ret += leg_IK((float*) Q, LEG_LF, (int16_t*) servoangles[0]);
-  //printf("leg_IK return %d\n", ret);
-  //print_int_matrix((int16_t*) servo_angles[0], 1, 3, "servo_angles LF", 1);
-
-
-
-  // printf("\n\nRF-----\n");
-  ret += inverse(Trf, inv);
-  // print_matrix((float*) Ix, 4, 4, "Ix");
-  // print_matrix((float*) inv, 4, 4, "inv Trf");
-  dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
-  // print_matrix((float*) Q1, 4, 4, "Q1 RF");
-  dspm_mult_f32_ae32((float*) Q1, (float*) Lp[1], (float*) Q, 4, 4, 1);
-  // print_matrix((float*) Q, 1, 4, "Q RF");
-  ret += leg_IK((float*) Q, LEG_RF, (int16_t*) servoangles[1]);
-  //printf("leg_IK return %d\n", ret);
-  //print_int_matrix((int16_t*) servo_angles[1], 1, 3, "servo_angles RF");
-
-  ret += inverse(Tlb, inv);
-  dspm_mult_f32_ae32((float*) inv, (float*) Lp[2], (float*) Q, 4, 4, 1);
-  // print_matrix((float*) Q, 1, 4, "Q LB");
-  ret += leg_IK((float*) Q, LEG_LB, (int16_t*) servoangles[2]);
-  //printf("leg_IK return %d\n", ret);
-  //print_int_matrix((int16_t*) servo_angles[2], 1, 3, "servo_angles LB");
-
-  // printf("\n\nRB-----\n");
-  ret += inverse(Trb, inv);
-  // print_matrix((float*) inv, 4, 4, "inv Trf");
-  dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
-  // print_matrix((float*) Q1, 4, 4, "Q1 RF");
-  dspm_mult_f32_ae32((float*) Q1, (float*) Lp[3], (float*) Q, 4, 4, 1);
-
-  // print_matrix((float*) Q, 1, 4, "Q RB");
-  ret += leg_IK((float*) Q, LEG_RB, (int16_t*) servoangles[3]);
-  //printf("leg_IK return %d\n", ret);
-  //print_int_matrix((int16_t*) servo_angles[3], 1, 3, "servo_angles RB");
-
-  return ret;
-}
@@ -0,0 +1,346 @@
+#include "globals.h"
+#include "webserver.h"
+
+// Maps settings for which a validator is available to the invocation thereof
+const std::map<String, CWebServer::ValueValidator> CWebServer::settingValidators
+{};
+
+std::vector<SettingSpec> CWebServer::deviceSettingSpecs{};
+
+// Member function template specialzations
+
+// Push param that represents a bool. Values considered true are text "true" and any whole number not equal to 0
+template<>
+bool CWebServer::PushPostParamIfPresent<bool>(AsyncWebServerRequest * pRequest, const String &paramName, ValueSetter<bool> setter)
+{
+    return PushPostParamIfPresent<bool>(pRequest, paramName, setter, [](AsyncWebParameter * param) constexpr
+    {
+        const String& value = param->value();
+        return value == "true" || strtol(value.c_str(), NULL, 10);
+    });
+}
+
+// Push param that represents a size_t
+template<>
+bool CWebServer::PushPostParamIfPresent<size_t>(AsyncWebServerRequest * pRequest, const String &paramName, ValueSetter<size_t> setter)
+{
+    return PushPostParamIfPresent<size_t>(pRequest, paramName, setter, [](AsyncWebParameter * param) constexpr
+    {
+        return strtoul(param->value().c_str(), NULL, 10);
+    });
+}
+
+// Add CORS header to and send JSON response
+template<>
+void CWebServer::AddCORSHeaderAndSendResponse<AsyncJsonResponse>(AsyncWebServerRequest * pRequest, AsyncJsonResponse * pResponse)
+{
+    pResponse->setLength();
+    AddCORSHeaderAndSendResponse<AsyncWebServerResponse>(pRequest, pResponse);
+}
+
+// Member function implementations
+
+bool CWebServer::IsPostParamTrue(AsyncWebServerRequest * pRequest, const String & paramName)
+{
+    bool returnValue = false;
+
+    PushPostParamIfPresent<bool>(pRequest, paramName, [&returnValue](auto value) { returnValue = value; return true; });
+
+    return returnValue;
+}
+
+void CWebServer::GetStatistics(AsyncWebServerRequest * pRequest)
+{
+    log_v("GetStatistics");
+
+    auto response = new AsyncJsonResponse(false, JSON_BUFFER_BASE_SIZE);
+    auto j = response->getRoot();
+
+    j["HEAP_SIZE"]             = _staticStats.HeapSize;
+    j["HEAP_FREE"]             = ESP.getFreeHeap();
+    j["HEAP_MIN"]              = ESP.getMinFreeHeap();
+
+    j["DMA_SIZE"]              = _staticStats.DmaHeapSize;
+    j["DMA_FREE"]              = heap_caps_get_free_size(MALLOC_CAP_DMA);
+    j["DMA_MIN"]               = heap_caps_get_largest_free_block(MALLOC_CAP_DMA);
+
+    j["PSRAM_SIZE"]            = _staticStats.PsramSize;
+    j["PSRAM_FREE"]            = ESP.getFreePsram();
+    j["PSRAM_MIN"]             = ESP.getMinFreePsram();
+
+    j["CHIP_MODEL"]            = _staticStats.ChipModel;
+    j["CHIP_CORES"]            = _staticStats.ChipCores;
+    j["CHIP_SPEED"]            = _staticStats.CpuFreqMHz;
+    j["PROG_SIZE"]             = _staticStats.SketchSize;
+
+    j["CODE_SIZE"]             = _staticStats.SketchSize;
+    j["CODE_FREE"]             = _staticStats.FreeSketchSpace;
+    j["FLASH_SIZE"]            = _staticStats.FlashChipSize;
+
+    j["CPU_USED"]              = g_TaskManager.GetCPUUsagePercent();
+    j["CPU_USED_CORE0"]        = g_TaskManager.GetCPUUsagePercent(0);
+    j["CPU_USED_CORE1"]        = g_TaskManager.GetCPUUsagePercent(1);
+
+    AddCORSHeaderAndSendResponse(pRequest, response);
+}
+
+void CWebServer::SendSettingSpecsResponse(AsyncWebServerRequest * pRequest, const std::vector<SettingSpec> & settingSpecs)
+{
+    static size_t jsonBufferSize = JSON_BUFFER_BASE_SIZE;
+    bool bufferOverflow;
+
+    do
+    {
+        bufferOverflow = false;
+        auto response = std::make_unique<AsyncJsonResponse>(false, jsonBufferSize);
+        auto jsonArray = response->getRoot().to<JsonArray>();
+
+        for (auto& spec : settingSpecs)
+        {
+            auto specObject = jsonArray.createNestedObject();
+
+            StaticJsonDocument<384> jsonDoc;
+
+            jsonDoc["name"] = spec.Name;
+            jsonDoc["friendlyName"] = spec.FriendlyName;
+            jsonDoc["description"] = spec.Description;
+            jsonDoc["type"] = to_value(spec.Type);
+            jsonDoc["typeName"] = spec.ToName(spec.Type);
+
+            if (!specObject.set(jsonDoc.as<JsonObjectConst>()))
+            {
+                bufferOverflow = true;
+                jsonBufferSize += JSON_BUFFER_INCREMENT;
+                log_v("JSON response buffer overflow! Increased buffer to %zu bytes", jsonBufferSize);
+                break;
+            }
+        }
+
+        if (!bufferOverflow)
+            AddCORSHeaderAndSendResponse(pRequest, response.release());
+
+    } while (bufferOverflow);
+}
+
+const std::vector<SettingSpec> & CWebServer::LoadDeviceSettingSpecs()
+{
+    if (deviceSettingSpecs.size() == 0)
+    {
+        auto deviceConfigSpecs = g_ptrDeviceConfig->GetSettingSpecs();
+        deviceSettingSpecs.insert(deviceSettingSpecs.end(), deviceConfigSpecs.begin(), deviceConfigSpecs.end());
+    }
+
+    return deviceSettingSpecs;
+}
+
+void CWebServer::GetSettingSpecs(AsyncWebServerRequest * pRequest)
+{
+    SendSettingSpecsResponse(pRequest, LoadDeviceSettingSpecs());
+}
+
+// Responds with current config, excluding any sensitive values
+void CWebServer::GetSettings(AsyncWebServerRequest * pRequest)
+{
+    log_v("GetSettings");
+
+    auto response = new AsyncJsonResponse(false, JSON_BUFFER_BASE_SIZE);
+    response->addHeader("Server","NightDriverStrip");
+    auto root = response->getRoot();
+    JsonObject jsonObject = root.to<JsonObject>();
+
+    // We get the serialized JSON for the device config, without any sensitive values
+    g_ptrDeviceConfig->SerializeToJSON(jsonObject, false);
+   
+    AddCORSHeaderAndSendResponse(pRequest, response);
+}
+
+// Support function that silently sets whatever settings are included in the request passed.
+//   Composing a response is left to the invoker!
+void CWebServer::SetSettingsIfPresent(AsyncWebServerRequest * pRequest)
+{
+    PushPostParamIfPresent<String>(pRequest, DeviceConfig::NTPServerTag, SET_VALUE(g_ptrDeviceConfig->SetNTPServer(value)));
+}
+
+// Set settings and return resulting config
+void CWebServer::SetSettings(AsyncWebServerRequest * pRequest)
+{
+    log_v("SetSettings");
+
+    SetSettingsIfPresent(pRequest);
+
+    // We return the current config in response
+    GetSettings(pRequest);
+}
+
+// Save the posted file to SPIFFS
+void CWebServer::SaveFile(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final)
+{
+    log_v("SaveFile");
+}
+
+#if USE_OAT
+// Update the current firmware
+void CWebServer::UpdateFileSystemImage(AsyncWebServerRequest *request, String filename, size_t index, uint8_t *data, size_t len, bool final)
+{
+    log_v("UpdateFileSystemImage");
+
+    HandleUpdate(request, filename, index, data, len, final, (ESP.getFreeSketchSpace() - 0x1000) & 0xFFFFF000, U_FLASH);
+}
+
+// Update the current firmware
+void CWebServer::UpdateFirmware(AsyncWebServerRequest *request, String filename, size_t index, uint8_t *data, size_t len, bool final)
+{
+    log_v("UpdateFirmware");
+
+    HandleUpdate(request, filename, index, data, len, final, UPDATE_SIZE_UNKNOWN, U_SPIFFS);
+}
+
+// Handles updating the filesystem image or firmware 
+void CWebServer::HandleUpdate(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final, size_t upload_size, uint8_t upload_index) {
+    if (!index) {
+        log_i("Update Start: %s\n", filename.c_str());
+        log_i("Uploading: %d", upload_index);
+        if (!Update.begin(upload_size, upload_index)) {
+            Update.printError(Serial);
+        }
+    }
+    if (!Update.hasError()) {
+        if (Update.write(data, len) != len) {
+            Update.printError(Serial);
+        }
+    }
+    if (final) {
+        if (Update.end(true)) {
+            log_i("Update Success: %uB\n", index + len);
+        }
+        else {
+            Update.printError(Serial);
+        }
+    }
+}
+
+// Ensures the request gets send to the client
+void CWebServer::UpdateRequestHandler(AsyncWebServerRequest* request){
+    bool success = !Update.hasError();
+    AsyncWebServerResponse* response = request->beginResponse(200, "text/plain", success ? "OK" : "FAIL");
+    response->addHeader("Connection", "close");
+    request->send(response);
+
+    if (success) {
+        delay(250);
+        ESP.restart();
+    }
+}
+#endif
+
+// Reset effect config, device config and/or the board itself
+void CWebServer::Reset(AsyncWebServerRequest * pRequest)
+{
+    if (IsPostParamTrue(pRequest, "deviceConfig"))
+    {
+        log_i("Removing DeviceConfig");
+        g_ptrDeviceConfig->RemovePersisted();
+    }
+
+    bool boardResetRequested = IsPostParamTrue(pRequest, "board");
+
+    AddCORSHeaderAndSendOKResponse(pRequest);
+
+    if (boardResetRequested)
+    {
+        log_w("Resetting device at API request!");
+        delay(1000);    // Give the response a second to be sent
+        throw new std::runtime_error("Resetting device at API request");
+    }
+}
+
+String dir(void) {
+   String x = "<table>";
+   File root = SPIFFS.open("/");
+   if(!root) {
+        return "Failed to open directory";
+    }
+    if(!root.isDirectory()) {
+        return "Not a directory";
+    }
+    File file = root.openNextFile();
+    while(file) {
+         x += "<tr><td>"+String(file.name());
+        if(file.isDirectory()){
+          x += "<td>DIR";
+        } else {
+          x += "<td style='text-align:right'>"+String(file.size());
+        }
+        file = root.openNextFile();
+    }
+    x += "<tr><td>Occupied space<td style='text-align:right'>"+String(SPIFFS.usedBytes());
+    x += "<tr><td>Total space<td style='text-align:right'>"+String(SPIFFS.totalBytes());
+    return x+"</table>";
+}
+
+// Send Gzip SPA
+void CWebServer::GzipSpa(AsyncWebServerRequest * pRequest)
+{
+    if(!SPIFFS.exists("/index.html.gz")){
+        log_e("Gzipped SPA not found");
+        pRequest->send(404, "text/html", dir());
+        return;
+    }
+
+    AsyncWebServerResponse *response = pRequest->beginResponse(SPIFFS, "/index.html.gz", "text/html", false);
+    response->addHeader("Content-Encoding", "gzip");
+    response->addHeader("Cache-Control"," max-age=86400");
+    AddCORSHeaderAndSendResponse(pRequest, response);
+}
+
+enum Action {
+    SERVO_UPDATE,
+};
+
+void handleWebSocketJson(uint8_t* data) {
+    const uint8_t size = JSON_OBJECT_SIZE(1);
+    StaticJsonDocument<100> json;
+    DeserializationError err = deserializeJson(json, data);
+    if (err) {
+        Serial.print(F("deserializeJson() failed with code "));
+        Serial.println(err.c_str());
+        return;
+    }
+
+    const uint8_t action = json["action"];
+
+    if(action == 0) {
+        const uint8_t servoIndex = json["servo"];
+        const int16_t pwm = json["pwm"];
+        log_i("Moving servo:%u to pwm%u", servoIndex, pwm);
+        g_ptrServo->setPWM(servoIndex, 0, pwm);
+    }
+}
+
+void handleWebSocketBuffer(uint8_t* data) {
+    if(data[0] == 0) g_ptrServo->setBody(data[1], data[2], data[3], data[4], data[5], data[6]);
+    else if(data[0] == 1) {
+        log_i("About to update all servos");
+        int8_t* angles = (int8_t*)data+1;  
+
+        g_ptrServo->SetAngles(angles);
+    }
+}
+
+void handleWebSocketMessage(void* arg, uint8_t* data, size_t len, AsyncWebSocket* server, AsyncWebSocketClient* client) {
+    AwsFrameInfo* info = (AwsFrameInfo*)arg;
+    if(info->final && info->index == 0 && info->len == len){
+        if(info->opcode == WS_TEXT) handleWebSocketJson(data);
+        else handleWebSocketBuffer(data);
+    }
+}
+
+// Reset effect config, device config and/or the board itself
+void CWebServer::HandleWsMessage(AsyncWebSocket* server, AsyncWebSocketClient* client, AwsEventType type, void* arg, uint8_t* data, size_t len)
+{
+    //if (type == WS_EVT_CONNECT) handleNewConnection(server, client);
+    /*else*/ if (type == WS_EVT_DISCONNECT) log_i("ws[%s][%u] disconnect\n", server->url(), client->id());
+    else if (type == WS_EVT_ERROR) log_i("ws[%s][%u] error(%u): %s\n", server->url(), client->id(), *((uint16_t*)arg), (char*)data);
+    else if (type == WS_EVT_PONG) log_i("ws[%s][%u] pong[%u]: %s\n", server->url(), client->id(), len, (len) ? (char*)data : "");
+    else if (type == WS_EVT_DATA) handleWebSocketMessage(arg, data, len, server, client);
+}