From b43c918c57f3f8e08888d99867a244771dc8e070 Mon Sep 17 00:00:00 2001 From: pulipakaa24 Date: Sun, 28 Dec 2025 20:52:11 -0600 Subject: [PATCH] Added basic calibration framework: pending servo integration and tying of encoders. --- include/calibration.cpp | 79 ++++++++ include/calibration.hpp | 25 +++ include/defines.h | 6 + include/encoder.hpp | 3 +- include/socketIO.cpp | 426 ++++++++++++++++++++++++---------------- include/socketIO.hpp | 6 +- src/main.cpp | 31 ++- 7 files changed, 398 insertions(+), 178 deletions(-) create mode 100644 include/calibration.cpp create mode 100644 include/calibration.hpp diff --git a/include/calibration.cpp b/include/calibration.cpp new file mode 100644 index 0000000..254b42d --- /dev/null +++ b/include/calibration.cpp @@ -0,0 +1,79 @@ +#include "calibration.hpp" +#include "defines.h" +#include "nvs_flash.h" + +void Calibration::init() { + nvs_handle_t calibHandle; + if (nvs_open(nvsCalib, NVS_READONLY, &calibHandle) == ESP_OK) { + int32_t tempTicks; + if (nvs_get_i32(calibHandle, UpMinusDownTicksTag, &tempTicks) == ESP_OK) { + uint8_t tempCalib; + if (nvs_get_u8(calibHandle, statusTag, &tempCalib) == ESP_OK) { + UpMinusDownTicks = tempTicks; + calibrated = tempCalib; + printf("Range: %d\n", tempTicks); + } + else { + printf("No status present\n"); + calibrated = false; + } + } + else { + printf("No Updownticks present\n"); + calibrated = false; + } + nvs_close(calibHandle); + } + else { + printf("CALIBINIT: failed to open NVS - not created?\n"); + calibrated = false; + } +} + +void Calibration::clearCalibrated() { + if (!calibrated) return; + + // clear variable and NVS + calibrated = false; + nvs_handle_t calibHandle; + if (nvs_open(nvsCalib, NVS_READWRITE, &calibHandle) == ESP_OK) { + if (nvs_set_u8(calibHandle, statusTag, false) != ESP_OK) + printf("Error saving calibration status as false.\n"); + nvs_close(calibHandle); + } + else printf("Error opening calibration NVS segment.\n"); +} + +bool Calibration::completeCalib() { + int32_t tempUpMinusDownTicks = startTicks - topEnc.getCount(); + if (calibrated && UpMinusDownTicks == tempUpMinusDownTicks) return true; + else { + nvs_handle_t calibHandle; + if (nvs_open(nvsCalib, NVS_READWRITE, &calibHandle) == ESP_OK) { + esp_err_t err = ESP_OK; + if (UpMinusDownTicks != tempUpMinusDownTicks) + err |= nvs_set_i32(calibHandle, UpMinusDownTicksTag, tempUpMinusDownTicks); + if (!calibrated) + err |= nvs_set_u8(calibHandle, statusTag, true); + if (err != ESP_OK) { + printf("Error saving calibration data.\n"); + return false; + } + UpMinusDownTicks = tempUpMinusDownTicks; + calibrated = true; + printf("Range: %d\n", tempUpMinusDownTicks); + nvs_close(calibHandle); + } + else { + printf("Error opening calibration NVS segment.\n"); + return false; + } + } + return true; +} + +int32_t Calibration::convertToTicks(int8_t steps10) { + // steps10 between -10 and +10 + // with +10 meaning full length upward, -10 meaning full length downward. + return ((int32_t)steps10 * UpMinusDownTicks) / 10; +} \ No newline at end of file diff --git a/include/calibration.hpp b/include/calibration.hpp new file mode 100644 index 0000000..c985775 --- /dev/null +++ b/include/calibration.hpp @@ -0,0 +1,25 @@ +#ifndef CALIBRATION_H +#define CALIBRATION_H +#include +#include "encoder.hpp" + +class Calibration { + public: + void init(); + void beginDownwardCalib() {startTicks = topEnc.getCount();} + bool completeCalib(); + int32_t convertToTicks(int8_t steps10); + bool getCalibrated() {return calibrated;} + void clearCalibrated(); + Calibration(Encoder& enc):topEnc(enc) {}; + + private: + std::atomic calibrated; + std::atomic UpMinusDownTicks; + int32_t startTicks; + Encoder& topEnc; +}; + +extern Calibration calib; + +#endif \ No newline at end of file diff --git a/include/defines.h b/include/defines.h index 49b216d..9edf6c8 100644 --- a/include/defines.h +++ b/include/defines.h @@ -8,14 +8,20 @@ #define ccwMax 10 #define cwMax 0 + #define nvsWiFi "WiFiCreds" #define ssidTag "SSID" #define passTag "PW" #define authTag "AuthMode" #define unameTag "UNAME" + #define nvsAuth "AUTH" #define tokenTag "TOKEN" +#define nvsCalib "CALIB" +#define UpMinusDownTicksTag "UPDOWN" +#define statusTag "STATUS" + #define ENCODER_PIN_A GPIO_NUM_23 #define ENCODER_PIN_B GPIO_NUM_16 diff --git a/include/encoder.hpp b/include/encoder.hpp index f55d805..ea0ffdb 100644 --- a/include/encoder.hpp +++ b/include/encoder.hpp @@ -1,11 +1,12 @@ #ifndef ENCODER_H #define ENCODER_H #include "driver/gpio.h" +#include class Encoder { public: // Shared between ISR and main code - volatile int32_t count; + std::atomic count; // ISR-only state uint8_t last_state_a; diff --git a/include/socketIO.cpp b/include/socketIO.cpp index 11e0db3..a5bfe0b 100644 --- a/include/socketIO.cpp +++ b/include/socketIO.cpp @@ -4,6 +4,7 @@ #include "WiFi.hpp" #include "setup.hpp" #include "cJSON.h" +#include "calibration.hpp" static esp_socketio_client_handle_t io_client; static esp_socketio_packet_handle_t tx_packet = NULL; @@ -14,195 +15,284 @@ std::atomic connected{false}; // Event handler for Socket.IO events static void socketio_event_handler(void *handler_args, esp_event_base_t base, int32_t event_id, void *event_data) { - esp_socketio_event_data_t *data = (esp_socketio_event_data_t *)event_data; - esp_socketio_packet_handle_t packet = data->socketio_packet; + esp_socketio_event_data_t *data = (esp_socketio_event_data_t *)event_data; + esp_socketio_packet_handle_t packet = data->socketio_packet; - switch (event_id) { - case SOCKETIO_EVENT_OPENED: - printf("Socket.IO Received OPEN packet\n"); - // Connect to default namespace "/" - esp_socketio_client_connect_nsp(data->client, NULL, NULL); - break; - - case SOCKETIO_EVENT_NS_CONNECTED: { - printf("Socket.IO Connected to namespace!\n"); - // Check if connected to default namespace - char *nsp = esp_socketio_packet_get_nsp(packet); - if (strcmp(nsp, "/") == 0) { - printf("Connected to default namespace - waiting for device_init...\n"); + switch (event_id) { + case SOCKETIO_EVENT_OPENED: + printf("Socket.IO Received OPEN packet\n"); + // Connect to default namespace "/" + esp_socketio_client_connect_nsp(data->client, NULL, NULL); + break; + + case SOCKETIO_EVENT_NS_CONNECTED: { + printf("Socket.IO Connected to namespace!\n"); + // Check if connected to default namespace + char *nsp = esp_socketio_packet_get_nsp(packet); + if (strcmp(nsp, "/") == 0) { + printf("Connected to default namespace - waiting for device_init...\n"); + } + // Don't set connected yet - wait for device_init message from server + break; + } + + case SOCKETIO_EVENT_DATA: { + printf("Received Socket.IO data\n"); + // Parse the received packet + cJSON *json = esp_socketio_packet_get_json(packet); + if (json) { + char *json_str = cJSON_Print(json); + printf("Data: %s\n", json_str); + + // Check if this is an array event + if (cJSON_IsArray(json) && cJSON_GetArraySize(json) >= 2) { + cJSON *eventName = cJSON_GetArrayItem(json, 0); + + if (cJSON_IsString(eventName)) { + // Handle error event + if (strcmp(eventName->valuestring, "error") == 0) { + printf("Received error message from server\n"); + cJSON *data = cJSON_GetArrayItem(json, 1); + + if (data) { + cJSON *message = cJSON_GetObjectItem(data, "message"); + if (message && cJSON_IsString(message)) { + printf("Server error: %s\n", message->valuestring); + } + } + + // Mark connection as failed + connected = false; + statusResolved = true; } - // Don't set connected yet - wait for device_init message from server - break; - } - - case SOCKETIO_EVENT_DATA: { - printf("Received Socket.IO data\n"); - // Parse the received packet - cJSON *json = esp_socketio_packet_get_json(packet); - if (json) { - char *json_str = cJSON_Print(json); - printf("Data: %s\n", json_str); - - // Check if this is an array event - if (cJSON_IsArray(json) && cJSON_GetArraySize(json) >= 2) { - cJSON *eventName = cJSON_GetArrayItem(json, 0); + // Handle device_init event + else if (strcmp(eventName->valuestring, "device_init") == 0) { + printf("Received device_init message\n"); + cJSON *data = cJSON_GetArrayItem(json, 1); + + if (data) { + cJSON *type = cJSON_GetObjectItem(data, "type"); + if (type && strcmp(type->valuestring, "success") == 0) { + printf("Device authenticated successfully\n"); + + // Parse device state + cJSON *deviceState = cJSON_GetObjectItem(data, "deviceState"); + if (cJSON_IsArray(deviceState)) { + int stateCount = cJSON_GetArraySize(deviceState); + printf("Device has %d peripheral(s):\n", stateCount); - if (cJSON_IsString(eventName)) { - // Handle error event - if (strcmp(eventName->valuestring, "error") == 0) { - printf("Received error message from server\n"); - cJSON *data = cJSON_GetArrayItem(json, 1); - - if (data) { - cJSON *message = cJSON_GetObjectItem(data, "message"); - if (message && cJSON_IsString(message)) { - printf("Server error: %s\n", message->valuestring); - } - } - - // Mark connection as failed - connected = false; - statusResolved = true; - } - // Handle device_init event - else if (strcmp(eventName->valuestring, "device_init") == 0) { - printf("Received device_init message\n"); - cJSON *data = cJSON_GetArrayItem(json, 1); - - if (data) { - cJSON *type = cJSON_GetObjectItem(data, "type"); - if (type && strcmp(type->valuestring, "success") == 0) { - printf("Device authenticated successfully\n"); - - // Parse device state - cJSON *deviceState = cJSON_GetObjectItem(data, "deviceState"); - if (cJSON_IsArray(deviceState)) { - int stateCount = cJSON_GetArraySize(deviceState); - printf("Device has %d peripheral(s):\n", stateCount); - - for (int i = 0; i < stateCount; i++) { - cJSON *periph = cJSON_GetArrayItem(deviceState, i); - int port = cJSON_GetObjectItem(periph, "port")->valueint; - int lastPos = cJSON_GetObjectItem(periph, "lastPos")->valueint; - bool awaitCalib = cJSON_IsTrue(cJSON_GetObjectItem(periph, "awaitCalib")); - bool calibrated = cJSON_IsTrue(cJSON_GetObjectItem(periph, "calibrated")); - // TODO: UPDATE MOTOR/ENCODER STATES BASED ON THIS, as well as the successive websocket updates. - printf(" Port %d: pos=%d, calibrated=%d, awaitCalib=%d\n", - port, lastPos, calibrated, awaitCalib); - } - } - - // Now mark as connected - connected = true; - statusResolved = true; - } else { - printf("Device authentication failed\n"); - connected = false; - statusResolved = true; - } - } - } - // Handle device_deleted event - else if (strcmp(eventName->valuestring, "device_deleted") == 0) { - printf("Device has been deleted from account - disconnecting\n"); - cJSON *data = cJSON_GetArrayItem(json, 1); - if (data) { - cJSON *message = cJSON_GetObjectItem(data, "message"); - if (message && cJSON_IsString(message)) { - printf("Server message: %s\n", message->valuestring); - } - } - connected = false; - statusResolved = true; - } + for (int i = 0; i < stateCount; i++) { + cJSON *periph = cJSON_GetArrayItem(deviceState, i); + int port = cJSON_GetObjectItem(periph, "port")->valueint; + int lastPos = cJSON_GetObjectItem(periph, "lastPos")->valueint; + bool calibrated = cJSON_IsTrue(cJSON_GetObjectItem(periph, "calibrated")); + // TODO: UPDATE MOTOR/ENCODER STATES BASED ON THIS, as well as the successive websocket updates. + printf(" Port %d: pos=%d, calibrated=%d, awaitCalib=%d\n", + port, lastPos, calibrated); + } + } + + // Now mark as connected + connected = true; + statusResolved = true; + } else { + printf("Device authentication failed\n"); + connected = false; + statusResolved = true; } - - free(json_str); + } } - break; - } - - case SOCKETIO_EVENT_ERROR: { - printf("Socket.IO Error!\n"); - - // Check WebSocket error details - esp_websocket_event_data_t *ws_event = data->websocket_event; - if (ws_event && ws_event->error_handle.esp_ws_handshake_status_code != 0) { - printf("HTTP Status: %d\n", ws_event->error_handle.esp_ws_handshake_status_code); - if (ws_event->error_handle.esp_ws_handshake_status_code == 401 || - ws_event->error_handle.esp_ws_handshake_status_code == 403) { - printf("Authentication failed - invalid token\n"); + // Handle device_deleted event + else if (strcmp(eventName->valuestring, "device_deleted") == 0) { + printf("Device has been deleted from account - disconnecting\n"); + cJSON *data = cJSON_GetArrayItem(json, 1); + if (data) { + cJSON *message = cJSON_GetObjectItem(data, "message"); + if (message && cJSON_IsString(message)) { + printf("Server message: %s\n", message->valuestring); } + } + connected = false; + statusResolved = true; + } + + // Handle calib_start event + else if (strcmp(eventName->valuestring, "calib_start") == 0) { + printf("Device calibration begun, setting up...\n"); + cJSON *data = cJSON_GetArrayItem(json, 1); + if (data) { + cJSON *port = cJSON_GetObjectItem(data, "port"); + if (port && cJSON_IsNumber(port)) { + if (port->valueint != 1) + printf("Error, non-1 port received for calibration\n"); + else { + calib.clearCalibrated(); + emitCalibStage1Ready(); + } + } + } } - // Set flags to indicate connection failure - connected = false; - statusResolved = true; - break; + // Handle user_stage1_complete event + else if (strcmp(eventName->valuestring, "user_stage1_complete") == 0) { + printf("User completed stage 1 (tilt up), switching direction...\n"); + cJSON *data = cJSON_GetArrayItem(json, 1); + if (data) { + cJSON *port = cJSON_GetObjectItem(data, "port"); + if (port && cJSON_IsNumber(port)) { + if (port->valueint != 1) + printf("Error, non-1 port received for calibration\n"); + else { + calib.beginDownwardCalib(); + emitCalibStage2Ready(); + } + } + } + } + + // Handle user_stage2_complete event + else if (strcmp(eventName->valuestring, "user_stage2_complete") == 0) { + printf("User completed stage 2 (tilt down), finalizing calibration...\n"); + cJSON *data = cJSON_GetArrayItem(json, 1); + if (data) { + cJSON *port = cJSON_GetObjectItem(data, "port"); + if (port && cJSON_IsNumber(port)) { + if (port->valueint != 1) + printf("Error, non-1 port received for calibration\n"); + else { + calib.completeCalib(); + emitCalibDone(); + } + } + } + } + } } + + free(json_str); + } + break; } - - // Handle WebSocket-level disconnections - if (data->websocket_event_id == WEBSOCKET_EVENT_DISCONNECTED) { - printf("WebSocket disconnected\n"); - connected = false; - statusResolved = true; + + case SOCKETIO_EVENT_ERROR: { + printf("Socket.IO Error!\n"); + + // Check WebSocket error details + esp_websocket_event_data_t *ws_event = data->websocket_event; + if (ws_event && ws_event->error_handle.esp_ws_handshake_status_code != 0) { + printf("HTTP Status: %d\n", ws_event->error_handle.esp_ws_handshake_status_code); + if (ws_event->error_handle.esp_ws_handshake_status_code == 401 || + ws_event->error_handle.esp_ws_handshake_status_code == 403) { + printf("Authentication failed - invalid token\n"); + } + } + + // Set flags to indicate connection failure + connected = false; + statusResolved = true; + break; } + } + + // Handle WebSocket-level disconnections + if (data->websocket_event_id == WEBSOCKET_EVENT_DISCONNECTED) { + printf("WebSocket disconnected\n"); + connected = false; + statusResolved = true; + } } void initSocketIO() { - // Prepare the Authorization Header (Bearer format) - std::string authHeader = "Authorization: Bearer " + webToken + "\r\n"; + // Prepare the Authorization Header (Bearer format) + std::string authHeader = "Authorization: Bearer " + webToken + "\r\n"; - statusResolved = false; - connected = false; - - esp_socketio_client_config_t config = {}; - config.websocket_config.uri = "ws://192.168.1.190:3000/socket.io/?EIO=4&transport=websocket"; - config.websocket_config.headers = authHeader.c_str(); - - io_client = esp_socketio_client_init(&config); - tx_packet = esp_socketio_client_get_tx_packet(io_client); - - esp_socketio_register_events(io_client, SOCKETIO_EVENT_ANY, socketio_event_handler, NULL); - esp_socketio_client_start(io_client); + statusResolved = false; + connected = false; + + esp_socketio_client_config_t config = {}; + config.websocket_config.uri = "ws://192.168.1.190:3000/socket.io/?EIO=4&transport=websocket"; + config.websocket_config.headers = authHeader.c_str(); + + io_client = esp_socketio_client_init(&config); + tx_packet = esp_socketio_client_get_tx_packet(io_client); + + esp_socketio_register_events(io_client, SOCKETIO_EVENT_ANY, socketio_event_handler, NULL); + esp_socketio_client_start(io_client); } void stopSocketIO() { - if (io_client != NULL) { - printf("Stopping Socket.IO client...\n"); - esp_socketio_client_close(io_client, pdMS_TO_TICKS(1000)); - esp_socketio_client_destroy(io_client); - io_client = NULL; - tx_packet = NULL; - connected = false; - statusResolved = false; - } + if (io_client != NULL) { + printf("Stopping Socket.IO client...\n"); + esp_socketio_client_close(io_client, pdMS_TO_TICKS(1000)); + esp_socketio_client_destroy(io_client); + io_client = NULL; + tx_packet = NULL; + connected = false; + statusResolved = false; + } } // Function to emit 'calib_done' as expected by your server void emitCalibDone(int port) { - // Set packet header: EIO MESSAGE type, SIO EVENT type, default namespace "/" - if (esp_socketio_packet_set_header(tx_packet, EIO_PACKET_TYPE_MESSAGE, - SIO_PACKET_TYPE_EVENT, NULL, -1) == ESP_OK) { - // Create JSON array with event name and data - cJSON *array = cJSON_CreateArray(); - cJSON_AddItemToArray(array, cJSON_CreateString("calib_done")); - - cJSON *data = cJSON_CreateObject(); - cJSON_AddNumberToObject(data, "port", port); - cJSON_AddItemToArray(array, data); - - // Set the JSON payload - esp_socketio_packet_set_json(tx_packet, array); - - // Send the packet - esp_socketio_client_send_data(io_client, tx_packet); - - // Reset packet for reuse - esp_socketio_packet_reset(tx_packet); - - cJSON_Delete(array); - } + // Set packet header: EIO MESSAGE type, SIO EVENT type, default namespace "/" + if (esp_socketio_packet_set_header(tx_packet, EIO_PACKET_TYPE_MESSAGE, + SIO_PACKET_TYPE_EVENT, NULL, -1) == ESP_OK) { + // Create JSON array with event name and data + cJSON *array = cJSON_CreateArray(); + cJSON_AddItemToArray(array, cJSON_CreateString("calib_done")); + + cJSON *data = cJSON_CreateObject(); + cJSON_AddNumberToObject(data, "port", port); + cJSON_AddItemToArray(array, data); + + // Set the JSON payload + esp_socketio_packet_set_json(tx_packet, array); + + // Send the packet + esp_socketio_client_send_data(io_client, tx_packet); + + // Reset packet for reuse + esp_socketio_packet_reset(tx_packet); + + cJSON_Delete(array); + } +} + +// Function to emit 'calib_stage1_ready' to notify server device is ready for tilt up +void emitCalibStage1Ready(int port) { + if (esp_socketio_packet_set_header(tx_packet, EIO_PACKET_TYPE_MESSAGE, + SIO_PACKET_TYPE_EVENT, NULL, -1) == ESP_OK) { + cJSON *array = cJSON_CreateArray(); + cJSON_AddItemToArray(array, cJSON_CreateString("calib_stage1_ready")); + + cJSON *data = cJSON_CreateObject(); + cJSON_AddNumberToObject(data, "port", port); + cJSON_AddItemToArray(array, data); + + esp_socketio_packet_set_json(tx_packet, array); + esp_socketio_client_send_data(io_client, tx_packet); + esp_socketio_packet_reset(tx_packet); + + cJSON_Delete(array); + } +} + +// Function to emit 'calib_stage2_ready' to notify server device is ready for tilt down +void emitCalibStage2Ready(int port) { + if (esp_socketio_packet_set_header(tx_packet, EIO_PACKET_TYPE_MESSAGE, + SIO_PACKET_TYPE_EVENT, NULL, -1) == ESP_OK) { + cJSON *array = cJSON_CreateArray(); + cJSON_AddItemToArray(array, cJSON_CreateString("calib_stage2_ready")); + + cJSON *data = cJSON_CreateObject(); + cJSON_AddNumberToObject(data, "port", port); + cJSON_AddItemToArray(array, data); + + esp_socketio_packet_set_json(tx_packet, array); + esp_socketio_client_send_data(io_client, tx_packet); + esp_socketio_packet_reset(tx_packet); + + cJSON_Delete(array); + } } \ No newline at end of file diff --git a/include/socketIO.hpp b/include/socketIO.hpp index de144f0..c8b0b9b 100644 --- a/include/socketIO.hpp +++ b/include/socketIO.hpp @@ -11,7 +11,9 @@ void initSocketIO(); // Stop and destroy Socket.IO client void stopSocketIO(); -// Emit calibration done event to server -void emitCalibDone(int port); +// Emit calibration stage events to server +void emitCalibStage1Ready(int port = 1); +void emitCalibStage2Ready(int port = 1); +void emitCalibDone(int port = 1); #endif // SOCKETIO_HPP \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 3947830..17bd35f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,6 +7,14 @@ #include "setup.hpp" #include "socketIO.hpp" #include "encoder.hpp" +#include "calibration.hpp" + +// Global encoder instances +Encoder topEnc(ENCODER_PIN_A, ENCODER_PIN_B); +Encoder bottomEnc(InputEnc_PIN_A, InputEnc_PIN_B); + +// Global calibration instance +Calibration calib(topEnc); void mainApp() { printf("Hello "); @@ -19,17 +27,21 @@ void mainApp() { ESP_ERROR_CHECK(ret); bmWiFi.init(); + calib.init(); - // Create and initialize encoder - Encoder encoder(ENCODER_PIN_A, ENCODER_PIN_B); - encoder.init(); + // Initialize encoders + topEnc.init(); + bottomEnc.init(); setupLoop(); statusResolved = false; + int32_t prevCount = topEnc.getCount(); + // Main loop while (1) { + // websocket disconnect/reconnect handling if (statusResolved) { if (!connected) { printf("Disconnected! Beginning setup loop.\n"); @@ -39,9 +51,14 @@ void mainApp() { else printf("Reconnected!\n"); statusResolved = false; } + // Your main application logic here - vTaskDelay(pdMS_TO_TICKS(1000)); - printf("loop\n"); + int32_t currentCount = topEnc.getCount(); + if (currentCount != prevCount) { + prevCount = currentCount; + printf("Encoder Pos: %d\n", prevCount); + } + vTaskDelay(pdMS_TO_TICKS(100)); } } @@ -63,6 +80,6 @@ void encoderTest() { } extern "C" void app_main() { - // mainApp(); - encoderTest(); + mainApp(); + // encoderTest(); } \ No newline at end of file