Added basic calibration framework: pending servo integration and tying of encoders.

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;
+}

include/calibration.hpp

@@ -0,0 +1,25 @@
+#ifndef CALIBRATION_H
+#define CALIBRATION_H
+#include <atomic>
+#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<bool> calibrated;
+    std::atomic<int32_t> UpMinusDownTicks;
+    int32_t startTicks;
+    Encoder& topEnc;
+};
+
+extern Calibration calib;
+
+#endif

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
 

include/encoder.hpp

@@ -1,11 +1,12 @@
 #ifndef ENCODER_H
 #define ENCODER_H
 #include "driver/gpio.h"
+#include <atomic>
 
 class Encoder {
 public:
     // Shared between ISR and main code
-    volatile int32_t count;
+    std::atomic<int32_t> count;
     
     // ISR-only state
     uint8_t last_state_a;

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;
@@ -84,11 +85,11 @@ static void socketio_event_handler(void *handler_args, esp_event_base_t base,
                       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);
+                              port, lastPos, calibrated);
+                      
                     }
                   }
                   
@@ -115,6 +116,57 @@ static void socketio_event_handler(void *handler_args, esp_event_base_t base,
               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();
+                  }
+                }
+              }
+            }
+            
+            // 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();
+                   }
+                }
+              }
+            }
           }
         }
         
@@ -206,3 +258,41 @@ void emitCalibDone(int port) {
     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);
+  }
+}

include/socketIO.hpp

@@ -11,7 +11,9 @@ void initSocketIO();
 // Stop and destroy Socket.IO client
 void stopSocketIO();
 
-// Emit calibration done event to server
+// Emit calibration stage events to server
-void emitCalibDone(int port);
+void emitCalibStage1Ready(int port = 1);
+void emitCalibStage2Ready(int port = 1);
+void emitCalibDone(int port = 1);
 
 #endif // SOCKETIO_HPP

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
+  // Initialize encoders
-  Encoder encoder(ENCODER_PIN_A, ENCODER_PIN_B);
+  topEnc.init();
-  encoder.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));
+    int32_t currentCount = topEnc.getCount();
-    printf("loop\n");
+    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();
+  mainApp();
-  encoderTest();
+  // encoderTest();
 }