adipu/Blinds_XIAO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
pulipakaa24
2025-12-31 15:44:48 -06:00
parent b43c918c57
commit 90e967c6f8
15 changed files with 3205 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/bmHTTP.hpp
View File

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

126
include/calibration.cpp
View File

@@ -5,21 +5,21 @@
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;
}
int32_t tempUpTicks;
int32_t tempDownTicks;
uint8_t tempCalib;
esp_err_t err = ESP_OK;
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 - %d\n", tempUpTicks, tempDownTicks);
}
else {
printf("No Updownticks present\n");
printf("Data missing from NVS\n");
calibrated = false;
}
nvs_close(calibHandle);
@@ -30,50 +30,80 @@ void Calibration::init() {
}
}
void Calibration::clearCalibrated() {
if (!calibrated) return;
bool Calibration::clearCalibrated() {
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;
}
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;
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;
}
bool Calibration::completeCalib(Encoder& topEnc) {
int32_t tempDownTicks = topEnc.getCount();
if (tempDownTicks == UpTicks) {
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);
}

15
include/calibration.hpp
View File

@@ -6,18 +6,17 @@
class Calibration {
public:
void init();
void beginDownwardCalib() {startTicks = topEnc.getCount();}
bool completeCalib();
int32_t convertToTicks(int8_t steps10);
bool beginDownwardCalib(Encoder& topEnc);
bool completeCalib(Encoder& topEnc);
int32_t convertToTicks(uint8_t appPos);
uint8_t convertToAppPos(int32_t ticks);
bool getCalibrated() {return calibrated;}
void clearCalibrated();
Calibration(Encoder& enc):topEnc(enc) {};
bool clearCalibrated();
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;

10
include/defines.h
View File

@@ -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))

107
include/encoder.cpp
View File

@@ -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";
@@ -13,47 +14,53 @@ Encoder::Encoder(gpio_num_t pinA, gpio_num_t pinB)
// Static ISR - receives Encoder instance via arg
void IRAM_ATTR Encoder::isr_handler(void* arg)
{
Encoder* encoder = static_cast<Encoder*>(arg);
Encoder* encoder = static_cast<Encoder*>(arg);
// Read GPIO levels directly from hardware
uint32_t gpio_levels = GPIO.in.val;
uint8_t current_a = (gpio_levels >> encoder->pin_a) & 0x1;
uint8_t current_b = (gpio_levels >> encoder->pin_b) & 0x1;
// Read GPIO levels directly from hardware
uint32_t gpio_levels = GPIO.in.val;
uint8_t current_a = (gpio_levels >> encoder->pin_a) & 0x1;
uint8_t current_b = (gpio_levels >> encoder->pin_b) & 0x1;
// Quadrature decoding logic
if (current_a != encoder->last_state_a) {
if (!current_a) {
if (current_b) encoder->last_count_base++;
else encoder->last_count_base--;
}
else {
if (current_b) encoder->last_count_base--;
else encoder->last_count_base++;
}
// Quadrature decoding logic
if (current_a != encoder->last_state_a) {
if (!current_a) {
if (current_b) encoder->last_count_base++;
else encoder->last_count_base--;
}
else if (current_b != encoder->last_state_b) {
if (!current_b) {
if (current_a) encoder->last_count_base--;
else encoder->last_count_base++;
}
else {
if (current_a) encoder->last_count_base++;
else encoder->last_count_base--;
}
else {
if (current_b) encoder->last_count_base--;
else encoder->last_count_base++;
}
}
else if (current_b != encoder->last_state_b) {
if (!current_b) {
if (current_a) encoder->last_count_base--;
else encoder->last_count_base++;
}
else {
if (current_a) encoder->last_count_base++;
else encoder->last_count_base--;
}
}
// Accumulate to full detent count
if (encoder->last_count_base > 3) {
encoder->count += 1;
encoder->last_count_base -= 4;
}
else if (encoder->last_count_base < 0) {
encoder->count -= 1;
encoder->last_count_base += 4;
}
// Accumulate to full detent count
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;
encoder->last_state_b = current_b;
encoder->last_state_a = current_a;
encoder->last_state_b = current_b;
}
void Encoder::init()
@@ -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;
}
}

46
include/encoder.hpp
View File

@@ -2,30 +2,42 @@
#define ENCODER_H
#include "driver/gpio.h"
#include <atomic>
#include "esp_timer.h"
class Encoder {
public:
// Shared between ISR and main code
std::atomic<int32_t> count;
// Shared between ISR and main code
std::atomic<int32_t> count;
// ISR-only state
uint8_t last_state_a;
uint8_t last_state_b;
int8_t last_count_base;
// ISR-only state
uint8_t last_state_a;
uint8_t last_state_b;
int8_t last_count_base;
// Configuration
gpio_num_t pin_a;
gpio_num_t pin_b;
// Configuration
gpio_num_t pin_a;
gpio_num_t pin_b;
// Static ISR that receives instance pointer via arg
static void isr_handler(void* arg);
// Static ISR that receives instance pointer via arg
static void isr_handler(void* arg);
// 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();
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

30
include/pwm.c
View File

@@ -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);
}

7
include/pwm.h
View File

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

237
include/servo.cpp Normal file
View File

@@ -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
}

43
include/servo.hpp Normal file
View File

@@ -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

163
include/socketIO.cpp
View File

@@ -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",
port, lastPos, calibrated);
printf(" Port %d: pos=%d\n", port, lastPos);
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();
emitCalibStage1Ready();
if (!servoInitCalib()) emitCalibError("Initialization failed");
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();
emitCalibStage2Ready();
if (!servoBeginDownwardCalib())
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 {
calib.completeCalib();
emitCalibDone();
}
emitCalibError("Non-1 port");
}
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
void emitCalibDone(int port) {
// Set packet header: EIO MESSAGE type, SIO EVENT type, default namespace "/"
// Helper function to emit Socket.IO event with data
static void emitSocketEvent(const char* eventName, cJSON* data) {
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, cJSON_CreateString(eventName));
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) {
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);
}
void emitCalibStage1Ready(int port = 1) {
cJSON *data = cJSON_CreateObject();
cJSON_AddNumberToObject(data, "port", port);
emitSocketEvent("calib_stage1_ready", data);
}
// 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);
}
void emitCalibStage2Ready(int port = 1) {
cJSON *data = cJSON_CreateObject();
cJSON_AddNumberToObject(data, "port", port);
emitSocketEvent("calib_stage2_ready", data);
}
// 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);
}

3
include/socketIO.hpp
View File

@@ -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

2
sdkconfig.seeed_xiao_esp32c6
View File

@@ -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)

2600
sdkconfig.seeed_xiao_esp32c6.old Normal file
View File

File diff suppressed because it is too large Load Diff

24
src/main.cpp
View File

@@ -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
int32_t currentCount = topEnc.getCount();
if (currentCount != prevCount) {
prevCount = currentCount;
printf("Encoder Pos: %d\n", prevCount);
if (clearCalibFlag) {
calib.clearCalibrated();
emitCalibStatus(false);
clearCalibFlag = false;
}
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));
}