diff --git a/.gitignore b/.gitignore index 89cc49c..0bc2b65 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch +data/ +include/secrets.h +lib/* \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 13132b4..2df96b0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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" ] } \ No newline at end of file diff --git a/app/src/Views/Stream.svelte b/app/src/Views/Stream.svelte index 90c1520..b54cce4 100644 --- a/app/src/Views/Stream.svelte +++ b/app/src/Views/Stream.svelte @@ -2,7 +2,7 @@ import { onDestroy } from 'svelte'; import location from '../lib/location'; - let videoStream = `//${location}/stream`; + let videoStream = `//${location}/api/stream`; onDestroy(() => { videoStream = '#'; diff --git a/app/src/components/Model/ModelView.svelte b/app/src/components/Model/ModelView.svelte index 8e9bafe..438fda9 100644 --- a/app/src/components/Model/ModelView.svelte +++ b/app/src/components/Model/ModelView.svelte @@ -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, 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); diff --git a/app/src/components/Topbar.svelte b/app/src/components/Topbar.svelte index 961fca2..c606468 100644 --- a/app/src/components/Topbar.svelte +++ b/app/src/components/Topbar.svelte @@ -31,7 +31,10 @@ -
{Math.floor($dataBuffer[5])}°🌡️
+
{Math.floor($dataBuffer[0])}°🌡️
+ {Math.floor($dataBuffer[9]*10)/10}V
+ {Math.floor($dataBuffer[8]*1000)/1000}A +
diff --git a/app/src/lib/socket.ts b/app/src/lib/socket.ts index 0362701..dec1db7 100644 --- a/app/src/lib/socket.ts +++ b/app/src/lib/socket.ts @@ -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)); } \ No newline at end of file diff --git a/app/src/routes/Config.svelte b/app/src/routes/Config.svelte index 6fb7997..557633f 100644 --- a/app/src/routes/Config.svelte +++ b/app/src/routes/Config.svelte @@ -25,6 +25,8 @@
Servo calibration +
Servo
+
diff --git a/lib/ServerHandlers/src/AsyncJpegStreamHandler.h b/include/AsyncJpegStream.h similarity index 100% rename from lib/ServerHandlers/src/AsyncJpegStreamHandler.h rename to include/AsyncJpegStream.h diff --git a/include/IK_config.h b/include/IK_config.h deleted file mode 100644 index 5e9cbc1..0000000 --- a/include/IK_config.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef IK_CONFIG_H -#define IK_CONFIG_H - -#include - -// 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 diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/include/camera.h b/include/camera.h new file mode 100644 index 0000000..da22dc9 --- /dev/null +++ b/include/camera.h @@ -0,0 +1,3 @@ +#pragma once + +esp_err_t InitializeCamera(); \ No newline at end of file diff --git a/include/deviceconfig.h b/include/deviceconfig.h new file mode 100644 index 0000000..22020c1 --- /dev/null +++ b/include/deviceconfig.h @@ -0,0 +1,151 @@ +#pragma once + +#include +#include +#include + +/* + * 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 settingSpecs; + size_t writerIndex; + + void SaveToJSON(); + + template + void SetAndSave(T& target, const T& source) + { + if (target == source) + return; + + target = source; + + SaveToJSON(); + } + + template + void SetIfPresentIn(const JsonObjectConst& jsonObject, T& target, const char *tag) + { + if (jsonObject.containsKey(tag)) + target = jsonObject[tag].as(); + } + + public: + + using ValidateResponse = std::pair; + + // 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()); + } + + 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& 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 g_ptrDeviceConfig; \ No newline at end of file diff --git a/include/featureflags.h b/include/featureflags.h new file mode 100644 index 0000000..d733e74 --- /dev/null +++ b/include/featureflags.h @@ -0,0 +1,31 @@ +#define USE_PSRAM true + +#define USE_WIFI true + +#define WAIT_FOR_WIFI false + +#define USE_WEBSERVER true + +#define USE_WEBSERVER_SSL false + +#define USE_WEBSOCKET true + +#define USE_OAT false + +#define USE_NTP false + +#define USE_MDNS true + +#define USE_DNS_SERVER false + +#define USE_REMOTE_SERIAL false + +#define USE_LOW_POWER false + +#define USE_CAMERA true + +#define USE_MPU true + +#define USE_POWER_BUTTON true + +#define USE_USS true \ No newline at end of file diff --git a/include/globals.h b/include/globals.h new file mode 100644 index 0000000..7554dcc --- /dev/null +++ b/include/globals.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include +#include +#include + +// Disable brownout problems +#include "soc/rtc_cntl_reg.h" +#include "soc/soc.h" + +/* + * Macros + */ +#define NAME_OF(x) #x + +/* + * Feature flags + */ + +#include + +#if USE_WIFI + #include +#endif + +#if USE_WIFI && USE_WEBSERVER + #if USE_WEBSERVER_SSL + #define ASYNC_TCP_SSL_ENABLED 1 + #include + #endif + #include +#endif + +#if USE_OAT + #include +#endif + +#if USE_DNS_SERVER + #include +#endif + +#if USE_MDNS + #include +#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 +#include +#include +#include + +#if USE_CAMERA + + #include +#endif + +#if USE_WIFI && USE_WEBSERVER + #include +#endif + +#if USE_WIFI + #include +#endif diff --git a/include/ik_task.h b/include/ik_task.h deleted file mode 100644 index f8bafca..0000000 --- a/include/ik_task.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef IK_TASK_H -#define IK_TASK_H - -#include -#include -#include -#include - -#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 diff --git a/include/jsonbase.h b/include/jsonbase.h new file mode 100644 index 0000000..3b40925 --- /dev/null +++ b/include/jsonbase.h @@ -0,0 +1,4 @@ +#pragma once + +#define JSON_BUFFER_BASE_SIZE 2048 +#define JSON_BUFFER_INCREMENT 2048 diff --git a/include/jsonserializer.h b/include/jsonserializer.h new file mode 100644 index 0000000..a06c067 --- /dev/null +++ b/include/jsonserializer.h @@ -0,0 +1,80 @@ +#pragma once + +#include +#include +#include + +struct IJSONSerializable +{ + virtual bool SerializeToJSON(JsonObject& jsonObject) = 0; + virtual bool DeserializeFromJSON(const JsonObjectConst& jsonObject) { return false; } +}; + +template +constexpr auto to_value(E e) noexcept +{ + return static_cast>(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 AllocatedJsonDocument; + +#else + typedef DynamicJsonDocument AllocatedJsonDocument; +#endif + +bool LoadJSONFile(const char *fileName, size_t& bufferSize, std::unique_ptr& 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 writer; + + WriterEntry(std::function writer) : + writer(writer) + {} + + WriterEntry(WriterEntry&& entry) : WriterEntry(entry.writer) + {} + }; + + std::vector 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 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 g_ptrJSONWriter; diff --git a/include/movement.h b/include/movement.h new file mode 100644 index 0000000..e4034cf --- /dev/null +++ b/include/movement.h @@ -0,0 +1,15 @@ +#pragma once + +#include + +float getHeading(); + +float getTemp(); + +float getAngleX(); + +float getAngleY(); + +float getAngleZ(); + +void IRAM_ATTR MovementHandlingLoopEntry(void *); \ No newline at end of file diff --git a/include/network.h b/include/network.h new file mode 100644 index 0000000..be380d6 --- /dev/null +++ b/include/network.h @@ -0,0 +1,5 @@ +#pragma once + +#include + +void IRAM_ATTR NetworkHandlingLoopEntry(void *); diff --git a/include/servo.h b/include/servo.h index 411578e..5692a1a 100644 --- a/include/servo.h +++ b/include/servo.h @@ -1,16 +1,114 @@ -#ifndef SERVO_H -#define SERVO_H +#pragma once -#include +#include +#include +#include +#include -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 \ No newline at end of file +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 g_ptrServo; diff --git a/include/servo_config.h b/include/servo_config.h deleted file mode 100644 index c789d16..0000000 --- a/include/servo_config.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/include/spot.h b/include/spot.h deleted file mode 100644 index a0f72b8..0000000 --- a/include/spot.h +++ /dev/null @@ -1,79 +0,0 @@ -#ifndef SPOT_h -#define SPOT_h - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Server functions -#include -#include - -// Disable brownout problems -#include "soc/rtc_cntl_reg.h" -#include "soc/soc.h" - -// Config -#include -#include - -#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 \ No newline at end of file diff --git a/include/spot_ik.h b/include/spot_ik.h deleted file mode 100644 index d340ff0..0000000 --- a/include/spot_ik.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/include/taskmanager.h b/include/taskmanager.h new file mode 100644 index 0000000..cebae3f --- /dev/null +++ b/include/taskmanager.h @@ -0,0 +1,215 @@ +#pragma once + +#include + +// 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; \ No newline at end of file diff --git a/include/types.h b/include/types.h new file mode 100644 index 0000000..2cdbd32 --- /dev/null +++ b/include/types.h @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include + +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]; + } +}; \ No newline at end of file diff --git a/include/webserver.h b/include/webserver.h new file mode 100644 index 0000000..7673a7f --- /dev/null +++ b/include/webserver.h @@ -0,0 +1,219 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 + using ParamValueGetter = std::function; + + // Template for value setting forwarding function, used by PushPostParamIfPresent() + template + using ValueSetter = std::function; + + // Value validating function type, as used by DeviceConfig (and possible others) + using ValueValidator = std::function; + + 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 deviceSettingSpecs; + static const std::map 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 + static bool PushPostParamIfPresent(AsyncWebServerRequest * pRequest, const String ¶mName, ValueSetter setter, ParamValueGetter 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 + static bool PushPostParamIfPresent(AsyncWebServerRequest * pRequest, const String ¶mName, ValueSetter setter) + { + return PushPostParamIfPresent(pRequest, paramName, setter, [](AsyncWebParameter * param) { return param->value(); }); + } + + // AddCORSHeaderAndSend(OK)Response + // + // Sends a response with CORS headers added + template + 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 & LoadDeviceSettingSpecs(); + static void SendSettingSpecsResponse(AsyncWebServerRequest * pRequest, const std::vector & 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; } \ No newline at end of file diff --git a/lib/ServerHandlers/src/WebsocketHandler.h b/lib/ServerHandlers/src/WebsocketHandler.h deleted file mode 100644 index 7b4994a..0000000 --- a/lib/ServerHandlers/src/WebsocketHandler.h +++ /dev/null @@ -1,15 +0,0 @@ -#include -#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); \ No newline at end of file diff --git a/lib/ServerHandlers/src/Websockethandler.cpp b/lib/ServerHandlers/src/Websockethandler.cpp deleted file mode 100644 index e7a7cc6..0000000 --- a/lib/ServerHandlers/src/Websockethandler.cpp +++ /dev/null @@ -1,75 +0,0 @@ -#include - -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); -} diff --git a/platformio.ini b/platformio.ini index 4b5e314..a56fec3 100644 --- a/platformio.ini +++ b/platformio.ini @@ -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 \ No newline at end of file +[env:esp32cam] +extends = base +board = esp32cam \ No newline at end of file diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..f4a2ab4 --- /dev/null +++ b/readme.md @@ -0,0 +1,281 @@ + +

+
+ + Markdownify +
+ Spot Micro - Leika +
+

+ +

A small quadruped robot, inspired by boston dynamic Spot.

+ + + +

+ Key Features • + Overview • + Getting started • + Credits • + Related • + License +

+ + + +## 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. + +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). + + + +## 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 + +Buy Me A Coffee + +## 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)  ·  +> GitHub [@runeharlyk](https://github.com/runeharlyk)  ·  +> LinkedIn [@Rune Harlyk](https://www.linkedin.com/in/rune-harlyk/) diff --git a/lib/ServerHandlers/src/AsyncJpegStreamHandler.cpp b/src/AsyncJpegStream.cpp similarity index 97% rename from lib/ServerHandlers/src/AsyncJpegStreamHandler.cpp rename to src/AsyncJpegStream.cpp index f7c0a25..9753f41 100644 --- a/lib/ServerHandlers/src/AsyncJpegStreamHandler.cpp +++ b/src/AsyncJpegStream.cpp @@ -1,4 +1,4 @@ -#include +#include 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); } \ No newline at end of file diff --git a/src/camera.cpp b/src/camera.cpp new file mode 100644 index 0000000..b4f5dac --- /dev/null +++ b/src/camera.cpp @@ -0,0 +1,43 @@ +#include +#include +#include + +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; +} \ No newline at end of file diff --git a/src/deviceconfig.cpp b/src/deviceconfig.cpp new file mode 100644 index 0000000..5c27efb --- /dev/null +++ b/src/deviceconfig.cpp @@ -0,0 +1,55 @@ +#include +#include +#include +#include +#include + +DRAM_ATTR std::unique_ptr 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 pJsonDoc(nullptr); + + if (LoadJSONFile(DEVICE_CONFIG_FILE, g_DeviceConfigJSONBufferSize, pJsonDoc)) + { + log_i("Loading DeviceConfig from JSON"); + + DeserializeFromJSON(pJsonDoc->as(), 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(); + } +} + diff --git a/src/ik_task.cpp b/src/ik_task.cpp deleted file mode 100644 index 0f2f201..0000000 --- a/src/ik_task.cpp +++ /dev/null @@ -1,139 +0,0 @@ -#include - -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); -} diff --git a/src/jsonserializer.cpp b/src/jsonserializer.cpp new file mode 100644 index 0000000..e836bee --- /dev/null +++ b/src/jsonserializer.cpp @@ -0,0 +1,177 @@ +#include +#include +#include +#include + +DRAM_ATTR std::unique_ptr g_ptrJSONWriter = nullptr; + +bool LoadJSONFile(const char *fileName, size_t& bufferSize, std::unique_ptr& 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& pJsonDoc, size_t& bufferSize, std::function serializationFunction) +{ + // Loop is here to deal with out of memory conditions + while(true) + { + pJsonDoc.reset(new AllocatedJsonDocument(bufferSize)); + JsonObject jsonObject = pJsonDoc->to(); + + 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 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 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(); + } + } +} diff --git a/src/main.cpp b/src/main.cpp index 74fd62f..a5921a6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,11 +1,32 @@ -#include +#include +#include -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(); + g_ptrDeviceConfig = std::make_unique(); + g_ptrServo = std::make_unique(); + + 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); } diff --git a/src/movement.cpp b/src/movement.cpp new file mode 100644 index 0000000..ab450bf --- /dev/null +++ b/src/movement.cpp @@ -0,0 +1,79 @@ +#include +#if USE_MPU + #include + #include + #include +#endif + +#if USE_POWER_BUTTON + #include +#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); + } +} \ No newline at end of file diff --git a/src/network.cpp b/src/network.cpp new file mode 100644 index 0000000..2aa1c43 --- /dev/null +++ b/src/network.cpp @@ -0,0 +1,96 @@ +#include +#include + +#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 \ No newline at end of file diff --git a/src/servo.cpp b/src/servo.cpp index 984497a..17dab5a 100644 --- a/src/servo.cpp +++ b/src/servo.cpp @@ -1,38 +1,3 @@ -#include +#include -#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; -} \ No newline at end of file +DRAM_ATTR std::unique_ptr g_ptrServo; \ No newline at end of file diff --git a/src/spot.cpp b/src/spot.cpp deleted file mode 100644 index 69c37aa..0000000 --- a/src/spot.cpp +++ /dev/null @@ -1,241 +0,0 @@ -#include - -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(); -} \ No newline at end of file diff --git a/src/spot_ik.cpp b/src/spot_ik.cpp deleted file mode 100644 index c1e616f..0000000 --- a/src/spot_ik.cpp +++ /dev/null @@ -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 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; -} \ No newline at end of file diff --git a/src/webserver.cpp b/src/webserver.cpp new file mode 100644 index 0000000..07757fe --- /dev/null +++ b/src/webserver.cpp @@ -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 CWebServer::settingValidators +{}; + +std::vector 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(AsyncWebServerRequest * pRequest, const String ¶mName, ValueSetter setter) +{ + return PushPostParamIfPresent(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(AsyncWebServerRequest * pRequest, const String ¶mName, ValueSetter setter) +{ + return PushPostParamIfPresent(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(AsyncWebServerRequest * pRequest, AsyncJsonResponse * pResponse) +{ + pResponse->setLength(); + AddCORSHeaderAndSendResponse(pRequest, pResponse); +} + +// Member function implementations + +bool CWebServer::IsPostParamTrue(AsyncWebServerRequest * pRequest, const String & paramName) +{ + bool returnValue = false; + + PushPostParamIfPresent(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 & settingSpecs) +{ + static size_t jsonBufferSize = JSON_BUFFER_BASE_SIZE; + bool bufferOverflow; + + do + { + bufferOverflow = false; + auto response = std::make_unique(false, jsonBufferSize); + auto jsonArray = response->getRoot().to(); + + 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())) + { + 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 & 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(); + + // 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(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 = ""; + 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 += "
"+String(file.name()); + if(file.isDirectory()){ + x += "DIR"; + } else { + x += ""+String(file.size()); + } + file = root.openNextFile(); + } + x += "
Occupied space"+String(SPIFFS.usedBytes()); + x += "
Total space"+String(SPIFFS.totalBytes()); + return x+"
"; +} + +// 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); +}