Added basically all servo logic with encoders. Need to test.

include/bmHTTP.hpp

@@ -1,5 +1,5 @@
-#ifndef BMHTTP
+#ifndef BMHTTP_H
-#define BMHTTP
+#define BMHTTP_H
 #include <string>
 #include "cJSON.h"
 

include/calibration.cpp

@@ -5,21 +5,21 @@
 void Calibration::init() {
   nvs_handle_t calibHandle;
   if (nvs_open(nvsCalib, NVS_READONLY, &calibHandle) == ESP_OK) {
-    int32_t tempTicks;
+    int32_t tempUpTicks;
-    if (nvs_get_i32(calibHandle, UpMinusDownTicksTag, &tempTicks) == ESP_OK) {
+    int32_t tempDownTicks;
     uint8_t tempCalib;
-      if (nvs_get_u8(calibHandle, statusTag, &tempCalib) == ESP_OK) {
+    esp_err_t err = ESP_OK;
-        UpMinusDownTicks = tempTicks;
+    err |= nvs_get_i32(calibHandle, UpTicksTag, &tempUpTicks);
+    err |= nvs_get_i32(calibHandle, DownTicksTag, &tempUpTicks);
+    err |= nvs_get_u8(calibHandle, statusTag, &tempCalib);
+    if (err == ESP_OK) {
+      UpTicks = tempUpTicks;
+      DownTicks = tempDownTicks;
       calibrated = tempCalib;
-        printf("Range: %d\n", tempTicks);
+      printf("Range: %d - %d\n", tempUpTicks, tempDownTicks);
     }
     else {
-        printf("No status present\n");
+      printf("Data missing from NVS\n");
-        calibrated = false;
-      }
-    }
-    else {
-      printf("No Updownticks present\n");
       calibrated = false;
     }
     nvs_close(calibHandle);
@@ -30,50 +30,80 @@ void Calibration::init() {
   }
 }
 
-void Calibration::clearCalibrated() {
+bool Calibration::clearCalibrated() {
-  if (!calibrated) return;
+  if (!calibrated) return true;
-  
   // 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)
+    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;
+}
+
+bool Calibration::beginDownwardCalib(Encoder& topEnc) {
+  int32_t tempUpTicks = topEnc.getCount();
+  nvs_handle_t calibHandle;
+  if (nvs_open(nvsCalib, NVS_READWRITE, &calibHandle) == ESP_OK) {
+    if (nvs_set_i32(calibHandle, UpTicksTag, tempUpTicks) == ESP_OK) {
+      printf("Saved UpTicks to NVS\n");
+      UpTicks = tempUpTicks;
+    }
+    else {
+      printf("Error saving UpTicks.\n");
+      return false;
+    }
+    nvs_close(calibHandle);
+  }
+  else {
+    printf("Error opening NVS to save UpTicks\n");
+    return false;
   }
   return true;
 }
 
-int32_t Calibration::convertToTicks(int8_t steps10) {
+bool Calibration::completeCalib(Encoder& topEnc) {
-  // steps10 between -10 and +10
+  int32_t tempDownTicks = topEnc.getCount();
-  // with +10 meaning full length upward, -10 meaning full length downward.
+  if (tempDownTicks == UpTicks) {
-  return ((int32_t)steps10 * UpMinusDownTicks) / 10;
+    printf("ERROR: NO RANGE\n");
+    return false;
+  }
+  nvs_handle_t calibHandle;
+  if (nvs_open(nvsCalib, NVS_READWRITE, &calibHandle) == ESP_OK) {
+    esp_err_t err = ESP_OK;
+    err |= nvs_set_i32(calibHandle, DownTicksTag, tempDownTicks);
+    err |= nvs_set_u8(calibHandle, statusTag, true);
+    if (err != ESP_OK) {
+      printf("Error saving calibration data.\n");
+      return false;
+    }
+    DownTicks = tempDownTicks;
+    calibrated = true;
+    printf("Range: %d - %d\n", UpTicks.load(), tempDownTicks);
+    nvs_close(calibHandle);
+  }
+  else {
+    printf("Error opening calibration NVS segment.\n");
+    return false;
+  }
+  return true;
+}
+
+int32_t Calibration::convertToTicks(uint8_t appPos) {
+  // appPos between 0 and 10, convert to target encoder ticks.
+  return (((int32_t)appPos * (UpTicks - DownTicks)) / 10) + DownTicks;
+}
+
+uint8_t Calibration::convertToAppPos(int32_t ticks) {
+  // appPos between 0 and 10, convert to target encoder ticks.
+  int8_t retVal = (ticks - DownTicks) * 10 / (UpTicks - DownTicks);
+  return (retVal < 0) ? 0 : ((retVal > 10) ? 10 : retVal);
 }

include/calibration.hpp

@@ -6,18 +6,17 @@
 class Calibration {
   public:
     void init();
-    void beginDownwardCalib() {startTicks = topEnc.getCount();}
+    bool beginDownwardCalib(Encoder& topEnc);
-    bool completeCalib();
+    bool completeCalib(Encoder& topEnc);
-    int32_t convertToTicks(int8_t steps10);
+    int32_t convertToTicks(uint8_t appPos);
+    uint8_t convertToAppPos(int32_t ticks);
     bool getCalibrated() {return calibrated;}
-    void clearCalibrated();
+    bool clearCalibrated();
-    Calibration(Encoder& enc):topEnc(enc) {};
+    std::atomic<int32_t> DownTicks;
+    std::atomic<int32_t> UpTicks;
 
   private:
     std::atomic<bool> calibrated;
-    std::atomic<int32_t> UpMinusDownTicks;
-    int32_t startTicks;
-    Encoder& topEnc;
 };
 
 extern Calibration calib;

include/defines.h

@@ -1,6 +1,7 @@
 #ifndef DEFINES_H
 #define DEFINES_H
 #include "driver/gpio.h"
+#include "driver/ledc.h"
 
 #define ccwSpeed 6500
 #define cwSpeed 3300
@@ -19,15 +20,22 @@
 #define tokenTag "TOKEN"
 
 #define nvsCalib "CALIB"
-#define UpMinusDownTicksTag "UPDOWN"
+#define UpTicksTag "UP"
+#define DownTicksTag "DOWN"
 #define statusTag "STATUS"
 
+#define nvsServo "SERVO"
+#define posTag "POS"
+
 #define ENCODER_PIN_A GPIO_NUM_23
 #define ENCODER_PIN_B GPIO_NUM_16
 
 #define InputEnc_PIN_A GPIO_NUM_1
 #define InputEnc_PIN_B GPIO_NUM_2
 
+#define servoPin GPIO_NUM_20
+#define servoLEDCChannel LEDC_CHANNEL_0
+
 #define getMovingCW(port) ((movingCW & (1 << port)) >> port)
 #define setMovingCW(port) (movingCW |= (1 << port))
 #define clearMovingCW(port) (movingCW &= ~(1 << port))

include/encoder.cpp

@@ -2,6 +2,7 @@
 #include "driver/gpio.h"
 #include "esp_log.h"
 #include "soc/gpio_struct.h"
+#include "servo.hpp"
 
 static const char *TAG = "ENCODER";
 
@@ -46,10 +47,16 @@ void IRAM_ATTR Encoder::isr_handler(void* arg)
   if (encoder->last_count_base > 3) {
     encoder->count += 1;
     encoder->last_count_base -= 4;
+    if (calibListen) servoCalibListen();
+    if (encoder->feedWDog) esp_timer_restart(encoder->watchdog_handle, 500000);
+    if (encoder->wandListen) servoWandListen();
   }
   else if (encoder->last_count_base < 0) {
     encoder->count -= 1;
     encoder->last_count_base += 4;
+    if (calibListen) servoCalibListen();
+    if (encoder->feedWDog) esp_timer_restart(encoder->watchdog_handle, 500000);
+    if (encoder->wandListen) servoWandListen();
   }
   
   encoder->last_state_a = current_a;
@@ -67,7 +74,7 @@ void Encoder::init()
     gpio_config(&io_conf);
 
     // Install ISR service if not already installed
-    gpio_install_isr_service(ESP_INTR_FLAG_IRAM);
+    gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1);
 
     // Attach ISR with THIS instance as argument
     gpio_isr_handler_add(pin_a, Encoder::isr_handler, this);
@@ -82,3 +89,31 @@ void Encoder::deinit()
     gpio_isr_handler_remove(pin_b);
     ESP_LOGI(TAG, "Encoder deinitialized");
 }
+
+void Encoder::setupWatchdog() {
+  if (watchdog_handle == NULL) {
+    const esp_timer_create_args_t enc_watchdog_args = {
+      .callback = &watchdogCallback,
+      .dispatch_method = ESP_TIMER_ISR,
+      .name = "encoder_wdt",
+    };
+
+    ESP_ERROR_CHECK(esp_timer_create(&enc_watchdog_args, &watchdog_handle));
+  }
+
+  ESP_ERROR_CHECK(esp_timer_start_once(watchdog_handle, 500000));
+  feedWDog = true;
+}
+
+void Encoder::pauseWatchdog() {
+  feedWDog = false;
+  esp_timer_stop(watchdog_handle);
+}
+
+Encoder::~Encoder() {
+  if (watchdog_handle != NULL) {
+    esp_timer_stop(watchdog_handle);
+    esp_timer_delete(watchdog_handle);
+    watchdog_handle = NULL;
+  }
+}

include/encoder.hpp

@@ -2,6 +2,7 @@
 #define ENCODER_H
 #include "driver/gpio.h"
 #include <atomic>
+#include "esp_timer.h"
 
 class Encoder {
 public:
@@ -20,12 +21,23 @@ public:
   // Static ISR that receives instance pointer via arg
   static void isr_handler(void* arg);
 
+  std::atomic<bool> feedWDog;
+  std::atomic<bool> serverListen;
+  std::atomic<bool> wandListen;
+
+  esp_timer_handle_t watchdog_handle;
+  
   // Constructor and methods
   Encoder(gpio_num_t pinA, gpio_num_t pinB);
   void init();
   int32_t getCount() const { return count; }
   void setCount(int32_t value) { count = value; }
   void deinit();
+
+  void setupWatchdog();
+  void pauseWatchdog();
+
+  ~Encoder();
 };
 
 #endif

include/pwm.c

@@ -1,30 +0,0 @@
-#include "driver/ledc.h"
-#include "defines.h"
-
-void init_servo_PWM(void) {
-    ledc_timer_config_t ledc_timer = {
-        .speed_mode       = LEDC_LOW_SPEED_MODE,
-        .timer_num        = LEDC_TIMER_0,
-        .duty_resolution  = LEDC_TIMER_16_BIT,
-        .freq_hz          = 50,
-        .clk_cfg          = LEDC_AUTO_CLK
-    };
-    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
-
-    ledc_channel_config_t ledc_channel = {
-        .speed_mode     = LEDC_LOW_SPEED_MODE,
-        .channel        = LEDC_CHANNEL_0,
-        .timer_sel      = LEDC_TIMER_0,
-        .intr_type      = LEDC_INTR_DISABLE,
-        .gpio_num       = 0, // Using pin D0 for servo.
-        .duty           = offSpeed, // Start off
-        .hpoint         = 0
-    };
-    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
-}
-
-void set_servo_speed(int channel, uint32_t duty) {
-    // duty input should be between 0 and 65535
-    ledc_set_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)channel, duty);
-    ledc_update_duty(LEDC_LOW_SPEED_MODE, (ledc_channel_t)channel);
-}

include/pwm.h

@@ -1,7 +0,0 @@
-#ifndef PWM_H
-#define PWM_H
-
-void init_servo_PWM(void);
-void set_servo_speed(int channel, int speed);
-
-#endif

include/servo.cpp

@@ -0,0 +1,237 @@
+#include "servo.hpp"
+#include "driver/ledc.h"
+#include "defines.h"
+#include <freertos/FreeRTOS.h>
+#include "esp_log.h"
+#include "socketIO.hpp"
+#include "nvs_flash.h"
+
+std::atomic<bool> calibListen{false};
+std::atomic<int32_t> baseDiff{0};
+std::atomic<int32_t> target{0};
+
+Encoder* topEnc = nullptr;
+Encoder* bottomEnc = nullptr;
+
+std::atomic<bool> runningManual{false};
+std::atomic<bool> runningServer{false};
+std::atomic<bool> clearCalibFlag{false};
+std::atomic<bool> savePosFlag{false};
+std::atomic<bool> startLess{false};
+
+void servoInit(Encoder& bottom, Encoder& top) {
+  // LEDC timer configuration (C++ aggregate initialization)
+  ledc_timer_config_t ledc_timer = {};
+  ledc_timer.speed_mode = LEDC_LOW_SPEED_MODE;
+  ledc_timer.timer_num = LEDC_TIMER_0;
+  ledc_timer.duty_resolution = LEDC_TIMER_16_BIT;
+  ledc_timer.freq_hz = 50;
+  ledc_timer.clk_cfg = LEDC_AUTO_CLK;
+  ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
+
+  // LEDC channel configuration
+  ledc_channel_config_t ledc_channel = {};
+  ledc_channel.speed_mode = LEDC_LOW_SPEED_MODE;
+  ledc_channel.channel = servoLEDCChannel;
+  ledc_channel.timer_sel = LEDC_TIMER_0;
+  ledc_channel.intr_type = LEDC_INTR_DISABLE;
+  ledc_channel.gpio_num = servoPin;
+  ledc_channel.duty = offSpeed; // Start off
+  ledc_channel.hpoint = 0;
+  ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
+
+  topEnc = &top;
+  bottomEnc = &bottom;
+
+  topEnc->count = servoReadPos();
+  if (calib.getCalibrated()) initMainLoop();
+}
+
+void servoOn(uint8_t dir, uint8_t manOrServer) {
+  servoMainSwitch(1);
+  ledc_set_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel, (dir ? ccwSpeed : cwSpeed));
+  ledc_update_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel);
+  runningManual = !manOrServer;
+  runningServer = manOrServer;
+}
+
+void servoOff() {
+  ledc_set_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel, offSpeed);
+  ledc_update_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel);
+  servoMainSwitch(0);
+  runningManual = false;
+  runningServer = false;
+}
+
+void servoMainSwitch(uint8_t onOff) {
+  // To Be Implemented
+}
+
+bool servoInitCalib() {
+  if (!calib.clearCalibrated()) return false;
+  if (topEnc == nullptr || bottomEnc == nullptr) {
+    printf("ERROR: CALIBRATION STARTED BEFORE SERVO INITIALIZATION\n");
+    return false;
+  }
+  baseDiff = bottomEnc->getCount() - topEnc->getCount();
+  calibListen = true;
+  return true;
+}
+
+void servoCalibListen() {
+  int32_t effDiff = (bottomEnc->getCount() - topEnc->getCount()) - baseDiff;
+  if (effDiff > 1) servoOn(CCW, manual);
+  else if (effDiff < -1) servoOn(CW, manual);
+  else servoOff();
+}
+
+bool servoBeginDownwardCalib() {
+  calibListen = false;
+  servoOff();
+  vTaskDelay(pdMS_TO_TICKS(1000));
+  if (!calib.beginDownwardCalib(*topEnc)) return false;
+  baseDiff = bottomEnc->getCount() - topEnc->getCount();
+  calibListen = true;
+  return true;
+}
+
+bool servoCompleteCalib() {
+  calibListen = false;
+  servoOff();
+  vTaskDelay(pdMS_TO_TICKS(1000));
+  if (!calib.completeCalib(*topEnc)) return false;
+  initMainLoop();
+  return true;
+}
+
+void initMainLoop() {
+  topEnc->setupWatchdog();
+  servoSavePos();
+  bottomEnc->wandListen = true;
+}
+
+void IRAM_ATTR watchdogCallback(void* arg) {
+  if (runningManual || runningServer) {
+    // if we're trying to move and our timer ran out, we need to recalibrate
+    clearCalibFlag = true;
+    topEnc->pauseWatchdog();
+
+    // get ready for recalibration by clearing all these listeners
+    bottomEnc->wandListen = false;
+    topEnc->wandListen = false;
+    topEnc->serverListen = false;
+    servoOff();
+  }
+  else {
+    // if no movement is running, we're fine
+    // save current servo-encoder position for reinitialization
+    savePosFlag = true;
+  }
+  // clear running flags
+  runningManual = false;
+  runningServer = false;
+}
+
+void servoSavePos() {
+  // save current servo-encoder position for use on reinitialization
+  nvs_handle_t servoHandle;
+  if (nvs_open(nvsServo, NVS_READWRITE, &servoHandle) == ESP_OK) {
+    int32_t topCount = topEnc->getCount();
+    if (nvs_set_i32(servoHandle, posTag, topCount) != ESP_OK)
+      printf("Error saving current position\n");
+    else printf("Success - Current position saved as: %d\n", topCount);
+    nvs_close(servoHandle);
+  }
+  else {
+    printf("Error opening servoPos NVS segment.\n");
+  }
+}
+
+int32_t servoReadPos() {
+  // save current servo-encoder position for use on reinitialization
+  int32_t val = 0;
+  nvs_handle_t servoHandle;
+  if (nvs_open(nvsServo, NVS_READONLY, &servoHandle) == ESP_OK) {
+    if (nvs_get_i32(servoHandle, posTag, &val) != ESP_OK)
+      printf("Error reading current position\n");
+    else printf("Success - Current position read as: %d\n", val);
+    nvs_close(servoHandle);
+  }
+  else {
+    printf("Error opening servoPos NVS segment.\n");
+  }
+  return val;
+}
+
+void stopServerRun() {
+  // stop listener and stop running if serverRun is still active.
+  topEnc->serverListen = false;
+  if (runningServer) servoOff();
+}
+
+void servoWandListen() {
+  // stop any remote-initiated movement
+  stopServerRun();
+
+  // freeze atomic values
+  int32_t upBound = calib.UpTicks;
+  int32_t downBound = calib.DownTicks;
+  int32_t bottomCount = bottomEnc->getCount();
+  int32_t topCount = topEnc->getCount();
+
+  // ensure the baseDiff doesn't wait on wand to turn all the way back to original range.
+  if ((upBound > downBound && bottomCount - baseDiff > upBound)
+       || (upBound < downBound && bottomCount - baseDiff < upBound))
+    baseDiff = bottomCount - upBound;
+  else if ((upBound > downBound && bottomCount - baseDiff < downBound)
+            || (upBound < downBound && bottomCount - baseDiff > downBound))
+    baseDiff = bottomCount - downBound;
+  
+  // calculate the difference between wand and top servo
+  int32_t effDiff = (bottomCount - topCount) - baseDiff;
+
+  // if we are at either bound, stop servo and servo-listener
+  // if effective difference is 0, stop servo and servo-listener
+  // otherwise, run servo in whichever direction necessary and
+  // ensure servo-listener is active.
+  if (abs(topCount - upBound) <= 1 || abs(topCount - downBound) <= 1) {
+    servoOff();
+    topEnc->wandListen = false;
+  }
+  else if (effDiff > 1) {
+    servoOn(CCW, manual);
+    topEnc->wandListen = true;
+  }
+  else if (effDiff < -1) {
+    servoOn(CW, manual);
+    topEnc->wandListen = true;
+  }
+  else {
+    servoOff();
+    topEnc->wandListen = false;
+  }
+}
+
+void servoServerListen() {
+  // If we have reached or passed our goal, stop running and stop listener.
+  if (topEnc->getCount() >= target && startLess) stopServerRun();
+  else if (topEnc->getCount() <= target && !startLess) stopServerRun();
+  baseDiff = bottomEnc->getCount() - topEnc->getCount();
+}
+
+void runToAppPos(uint8_t appPos) {
+  // manual control takes precedence over remote control, always.
+  if (runningManual) return;
+  servoOff();
+
+  // allow servo position to settle
+  vTaskDelay(pdMS_TO_TICKS(500));
+  int32_t topCount = topEnc->getCount();
+  target = calib.convertToTicks(appPos); // calculate target encoder position
+  if (abs(topCount - target) <= 1) return;
+  startLess = topCount < target;
+  if (runningManual) return; // check again before starting remote control
+  if (startLess) servoOn(CCW, server); // begin servo movement
+  else servoOn(CW, server);
+  topEnc->serverListen = true; // start listening for shutoff point
+}

include/servo.hpp

@@ -0,0 +1,43 @@
+#ifndef SERVO_H
+#define SERVO_H
+#include <atomic>
+#include "calibration.hpp"
+#include "encoder.hpp"
+
+#define CCW 1
+#define CW 0
+#define server 1
+#define manual 0
+
+extern Encoder* topEnc;
+extern Encoder* bottomEnc;
+extern std::atomic<bool> calibListen;
+extern std::atomic<bool> runningManual;
+extern std::atomic<bool> runningServer;
+extern std::atomic<bool> clearCalibFlag;
+extern std::atomic<bool> savePosFlag;
+extern std::atomic<bool> startLess;
+
+extern std::atomic<int32_t> baseDiff;
+extern std::atomic<int32_t> target;
+
+void servoInit(Encoder& bottom, Encoder& top);
+void servoOn(uint8_t dir, uint8_t manOrServer);
+void servoOff();
+void servoMainSwitch(uint8_t onOff);
+void servoSavePos();
+void servoCalibListen();
+bool servoInitCalib();
+bool servoBeginDownwardCalib();
+bool servoCompleteCalib();
+
+void initMainLoop();
+void watchdogCallback(void* arg);
+void servoSavePos();
+int32_t servoReadPos();
+void stopServerRun();
+void servoWandListen();
+
+void runToAppPos(uint8_t appPos);
+
+#endif

include/socketIO.cpp

@@ -5,6 +5,7 @@
 #include "setup.hpp"
 #include "cJSON.h"
 #include "calibration.hpp"
+#include "servo.hpp"
 
 static esp_socketio_client_handle_t io_client;
 static esp_socketio_packet_handle_t tx_packet = NULL;
@@ -85,11 +86,15 @@ 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 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",
+                      printf("  Port %d: pos=%d\n", port, lastPos);
-                              port, lastPos, calibrated);
+                      if (port != 1) printf("ERROR: NON-1 PORT RECEIVED\n");
-                      
+                      // Report back actual calibration status from device
+                      else {
+                        bool deviceCalibrated = calib.getCalibrated();
+                        emitCalibStatus(deviceCalibrated);
+                        printf("  Reported calibrated=%d for port %d\n", deviceCalibrated, port);
+                      }
                     }
                   }
                   
@@ -124,11 +129,13 @@ static void socketio_event_handler(void *handler_args, esp_event_base_t base,
               if (data) {
                 cJSON *port = cJSON_GetObjectItem(data, "port");
                 if (port && cJSON_IsNumber(port)) {
-                  if (port->valueint != 1)
+                  if (port->valueint != 1) {
                     printf("Error, non-1 port received for calibration\n");
+                    emitCalibError("Non-1 Port");
+                  }
                   else {
-                    calib.clearCalibrated();
+                    if (!servoInitCalib()) emitCalibError("Initialization failed");
-                    emitCalibStage1Ready();
+                    else emitCalibStage1Ready();
                   }
                 }
               }
@@ -141,11 +148,14 @@ static void socketio_event_handler(void *handler_args, esp_event_base_t base,
               if (data) {
                 cJSON *port = cJSON_GetObjectItem(data, "port");
                 if (port && cJSON_IsNumber(port)) {
-                  if (port->valueint != 1)
+                  if (port->valueint != 1) {
                     printf("Error, non-1 port received for calibration\n");
+                    emitCalibError("Non-1 Port");
+                  }
                    else {
-                    calib.beginDownwardCalib();
+                    if (!servoBeginDownwardCalib())
-                    emitCalibStage2Ready();
+                      emitCalibError("Direction Switch Failed");
+                    else emitCalibStage2Ready();
                    }
                 }
               }
@@ -158,12 +168,45 @@ static void socketio_event_handler(void *handler_args, esp_event_base_t base,
               if (data) {
                 cJSON *port = cJSON_GetObjectItem(data, "port");
                 if (port && cJSON_IsNumber(port)) {
-                  if (port->valueint != 1)
+                  if (port->valueint != 1) {
                     printf("Error, non-1 port received for calibration\n");
-                   else {
+                    emitCalibError("Non-1 port");
-                    calib.completeCalib();
-                    emitCalibDone();
                   }
+                  else {
+                    if (!servoCompleteCalib()) emitCalibError("Completion failed");
+                    else emitCalibDone();
+                  }
+                }
+              }
+            }
+            
+            // Handle server position change (manual or scheduled)
+            else if (strcmp(eventName->valuestring, "posUpdates") == 0) {
+              printf("Received position update from server\n");
+              cJSON *updateList = cJSON_GetArrayItem(json, 1);
+              
+              if (cJSON_IsArray(updateList)) {
+                int updateCount = cJSON_GetArraySize(updateList);
+                printf("Processing %d position update(s)\n", updateCount);
+                
+                for (int i = 0; i < updateCount; i++) {
+                  cJSON *update = cJSON_GetArrayItem(updateList, i);
+                  cJSON *periphNum = cJSON_GetObjectItem(update, "periphNum");
+                  cJSON *pos = cJSON_GetObjectItem(update, "pos");
+                  
+                  if (periphNum && cJSON_IsNumber(periphNum) && 
+                      pos && cJSON_IsNumber(pos)) {
+                    int port = periphNum->valueint;
+                    int position = pos->valueint;
+                    
+                    if (port != 1)
+                      printf("ERROR: Received position update for non-1 port: %d\n", port);
+                    else {
+                      printf("Position update: position %d\n", position);
+                      runToAppPos(position);
+                    }
+                  } 
+                  else printf("Invalid position update format\n");
                 }
               }
             }
@@ -233,66 +276,66 @@ void stopSocketIO() {
   }
 }
 
-// Function to emit 'calib_done' as expected by your server
+// Helper function to emit Socket.IO event with data
-void emitCalibDone(int port) {
+static void emitSocketEvent(const char* eventName, cJSON* data) {
-  // 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_AddItemToArray(array, cJSON_CreateString(eventName));
-    
-    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);
+  } else {
+    // If packet header setup failed, clean up the data object
+    cJSON_Delete(data);
   }
 }
 
+// Function to emit 'calib_done' as expected by your server
+void emitCalibDone(int port = 1) {
+  cJSON *data = cJSON_CreateObject();
+  cJSON_AddNumberToObject(data, "port", port);
+  emitSocketEvent("calib_done", data);
+}
+
 // Function to emit 'calib_stage1_ready' to notify server device is ready for tilt up
-void emitCalibStage1Ready(int port) {
+void emitCalibStage1Ready(int port = 1) {
-  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);
+  emitSocketEvent("calib_stage1_ready", 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) {
+void emitCalibStage2Ready(int port = 1) {
-  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);
+  emitSocketEvent("calib_stage2_ready", 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 'report_calib_status' to tell server device's actual calibration state
+void emitCalibStatus(bool calibrated, int port = 1) {
+  cJSON *data = cJSON_CreateObject();
+  cJSON_AddNumberToObject(data, "port", port);
+  cJSON_AddBoolToObject(data, "calibrated", calibrated);
+  emitSocketEvent("report_calib_status", data);
+}
+
+// Function to emit 'device_calib_error' to notify server of calibration failure
+void emitCalibError(const char* errorMessage, int port = 1) {
+  cJSON *data = cJSON_CreateObject();
+  cJSON_AddNumberToObject(data, "port", port);
+  cJSON_AddStringToObject(data, "message", errorMessage);
+  emitSocketEvent("device_calib_error", data);
+}
+
+// Function to emit 'pos_hit' to notify server of position change
+void emitPosHit(int pos, int port = 1) {
+  cJSON *data = cJSON_CreateObject();
+  cJSON_AddNumberToObject(data, "port", port);
+  cJSON_AddNumberToObject(data, "pos", pos);
+  emitSocketEvent("pos_hit", data);
 }

include/socketIO.hpp

@@ -12,8 +12,11 @@ void initSocketIO();
 void stopSocketIO();
 
 // Emit calibration stage events to server
+void emitCalibStatus(bool calibrated, int port = 1);
 void emitCalibStage1Ready(int port = 1);
 void emitCalibStage2Ready(int port = 1);
 void emitCalibDone(int port = 1);
+void emitCalibError(const char* errorMessage, int port = 1);
+void emitPosHit(int pos, int port = 1);
 
 #endif // SOCKETIO_HPP

sdkconfig.seeed_xiao_esp32c6

@@ -1664,7 +1664,7 @@ CONFIG_ESP_TIMER_INTERRUPT_LEVEL=1
 CONFIG_ESP_TIMER_TASK_AFFINITY=0x0
 CONFIG_ESP_TIMER_TASK_AFFINITY_CPU0=y
 CONFIG_ESP_TIMER_ISR_AFFINITY_CPU0=y
-# CONFIG_ESP_TIMER_SUPPORTS_ISR_DISPATCH_METHOD is not set
+CONFIG_ESP_TIMER_SUPPORTS_ISR_DISPATCH_METHOD=y
 CONFIG_ESP_TIMER_IMPL_SYSTIMER=y
 # end of ESP Timer (High Resolution Timer)
 

src/main.cpp

@@ -1,5 +1,5 @@
 #include <driver/gptimer.h>
-#include "pwm.h"
+#include "servo.hpp"
 #include "defines.h"
 #include "nvs_flash.h"
 #include "NimBLEDevice.h"
@@ -14,7 +14,7 @@ Encoder topEnc(ENCODER_PIN_A, ENCODER_PIN_B);
 Encoder bottomEnc(InputEnc_PIN_A, InputEnc_PIN_B);
 
 // Global calibration instance
-Calibration calib(topEnc);
+Calibration calib;
 
 void mainApp() {
   printf("Hello ");
@@ -32,6 +32,7 @@ void mainApp() {
   // Initialize encoders
   topEnc.init();
   bottomEnc.init();
+  servoInit(bottomEnc, topEnc);
 
   setupLoop();
   
@@ -52,11 +53,20 @@ void mainApp() {
       statusResolved = false;
     }
 
-    // Your main application logic here
+    if (clearCalibFlag) {
-    int32_t currentCount = topEnc.getCount();
+      calib.clearCalibrated();
-    if (currentCount != prevCount) {
+      emitCalibStatus(false);
-      prevCount = currentCount;
+      clearCalibFlag = false;
-      printf("Encoder Pos: %d\n", prevCount);
+    }
+    if (savePosFlag) {
+      servoSavePos();
+      savePosFlag = false;
+
+      // Send position update to server
+      uint8_t currentAppPos = calib.convertToAppPos(topEnc.getCount());
+      emitPosHit(currentAppPos);
+      
+      printf("Sent pos_hit: position %d\n", currentAppPos);
     }
     vTaskDelay(pdMS_TO_TICKS(100));
   }