diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts index ab2cbd6..5114471 100644 --- a/app/src/lib/stores/model-store.ts +++ b/app/src/lib/stores/model-store.ts @@ -8,12 +8,14 @@ export const jointNames = persistentStore('joint_names', []); export const model = writable(); -export const modes = ['idle', 'rest', 'stand', 'Crawl', 'walk'] as const; +export const modes = ['deactivated', 'idle', 'calibration', 'rest', 'stand', 'crawl', 'walk'] as const; export type Modes = (typeof modes)[number]; export enum ModesEnum { + Deactivated, Idle, + Calibration, Rest, Stand, Crawl, diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp index ed7c5e3..45606a6 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp +++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp @@ -59,7 +59,7 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server, unsigned int numberEnd _fileExplorer(server, &_securitySettingsService), _servoController(server, &ESPFS, &_securitySettingsService, &_peripherals, &_socket), #if FT_ENABLED(FT_MOTION) - _motionService(_server, &_socket, &_securitySettingsService, &_taskManager), + _motionService(_server, &_socket, &_securitySettingsService, &_servoController, &_taskManager), #endif #if FT_ENABLED(FT_WS2812) _ledService(&_taskManager), @@ -208,9 +208,6 @@ void IRAM_ATTR ESP32SvelteKit::loop() { #if FT_ENABLED(FT_BATTERY) _batteryService.loop(); #endif -#if FT_ENABLED(FT_MOTION) - _motionService.loop(); -#endif #if FT_ENABLED(FT_WS2812) _ledService.loop(); #endif diff --git a/esp32/lib/ESP32-sveltekit/Kinematics.h b/esp32/lib/ESP32-sveltekit/Kinematics.h index 2710066..39f058f 100644 --- a/esp32/lib/ESP32-sveltekit/Kinematics.h +++ b/esp32/lib/ESP32-sveltekit/Kinematics.h @@ -51,13 +51,13 @@ class Kinematics { float L, W; Kinematics() { - l1 = 50; - l2 = 20; - l3 = 120; - l4 = 155; + l1 = 60.5 / 100; + l2 = 10 / 100; + l3 = 100.7 / 100; + l4 = 118.5 / 100; - L = 140; - W = 75; + L = 207.5 / 100; + W = 78 / 100; } ~Kinematics() {} @@ -135,7 +135,8 @@ class Kinematics { float H = sqrtf(G * G + z * z); float theta1 = -atan2f(y, x) - atan2f(F, -l1); - float theta3 = acosf((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4)); + float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4); + float theta3 = atan2(sqrt(1 - D * D), D); if (isnan(theta3)) theta3 = 0; float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3)); result[0] = RAD_TO_DEG_F(theta1); diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h index eb7b6c8..38d276a 100644 --- a/esp32/lib/ESP32-sveltekit/MotionService.h +++ b/esp32/lib/ESP32-sveltekit/MotionService.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -15,13 +16,17 @@ #define POSITION_EVENT "position" #define MODE_EVENT "mode" -enum class MOTION_STATE { IDLE, REST, STAND, WALK }; +enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, CRAWL, WALK }; class MotionService { public: MotionService(PsychicHttpServer *server, EventSocket *socket, SecurityManager *securityManager, - TaskManager *taskManager) - : _server(server), _socket(socket), _securityManager(securityManager), _taskManager(taskManager) {} + ServoController *servoController, TaskManager *taskManager) + : _server(server), + _socket(socket), + _securityManager(securityManager), + _servoController(servoController), + _taskManager(taskManager) {} void begin() { _socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); }); @@ -36,6 +41,8 @@ class MotionService { std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2)); body_state.updateFeet(default_feet_positions); + + _taskManager->createTask(this->_loopImpl, "MotionService", 4096, this, 3); } void anglesEvent(JsonObject &root, int originId) { @@ -58,53 +65,60 @@ class MotionService { void handleInput(JsonObject &root, int originId) { JsonArray array = root["data"].as(); - float lx = array[1]; - float ly = array[2]; - float rx = array[3]; - float ry = array[4]; - float h = array[5]; - float s = array[6]; + command.lx = array[1]; + command.lx = array[1]; + command.ly = array[2]; + command.rx = array[3]; + command.ry = array[4]; + command.h = array[5]; + command.s = array[6]; - body_state.ym = (h + 128) * (float)0.7; + body_state.ym = (command.h + 128.f) * 0.75f / 100; switch (motionState) { case MOTION_STATE::STAND: { - body_state.phi = rx / 4; - body_state.psi = ry / 4; - body_state.xm = ly / 2; - body_state.zm = lx / 2; + body_state.phi = command.rx / 4; + body_state.psi = command.ry / 4; + body_state.xm = command.ly / 2 / 100; + body_state.zm = command.lx / 2 / 100; break; } } } void handleMode(JsonObject &root, int originId) { - ESP_LOGV("MotionService", "Mode %d", root["data"].as()); motionState = (MOTION_STATE)root["data"].as(); + ESP_LOGV("MotionService", "Mode %d", motionState); char output[2]; - sprintf(output, "%d", (int)motionState); + itoa((int)motionState, output, 10); + motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate(); _socket->emit(MODE_EVENT, output, String(originId).c_str()); } void syncAngles(const String &originId = "", bool sync = false) { char output[100]; - sprintf(output, "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0], angles[1], - angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], - angles[11]); + snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0], + angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], + angles[10], angles[11]); _socket->emit(ANGLES_EVENT, output, originId.c_str()); + _servoController->setAngles(angles); } bool updateMotion() { switch (motionState) { - case MOTION_STATE::IDLE: return false; break; - + case MOTION_STATE::DEACTIVATED: return false; + case MOTION_STATE::IDLE: return false; + case MOTION_STATE::CALIBRATION: update_angles(calibration_angles, new_angles, false); break; case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break; - - case MOTION_STATE::STAND: { + case MOTION_STATE::STAND: kinematics.calculate_inverse_kinematics(body_state, new_angles); break; + case MOTION_STATE::CRAWL: + crawlGait->step(body_state, command); + kinematics.calculate_inverse_kinematics(body_state, new_angles); + break; + case MOTION_STATE::WALK: + walkGait->step(body_state, command); kinematics.calculate_inverse_kinematics(body_state, new_angles); break; - } - case MOTION_STATE::WALK: kinematics.calculate_inverse_kinematics(body_state, new_angles); break; } return update_angles(new_angles, angles); } @@ -121,38 +135,44 @@ class MotionService { return updated; } - void loop() { - EXECUTE_EVERY_N_MS(MotionInterval, { + void _loop() { + TickType_t xLastWakeTime = xTaskGetTickCount(); + for (;;) { if (updateMotion()) syncAngles(); - }); + _servoController->loop(); + vTaskDelayUntil(&xLastWakeTime, MotionInterval / portTICK_PERIOD_MS); + } } + static void _loopImpl(void *_this) { static_cast(_this)->_loop(); } + private: PsychicHttpServer *_server; EventSocket *_socket; SecurityManager *_securityManager; TaskManager *_taskManager; + ServoController *_servoController; Kinematics kinematics; + ControllerCommand command = {0, 0, 0, 0, 0, 0, 0}; - MOTION_STATE motionState = MOTION_STATE::IDLE; + friend class GaitState; + + std::unique_ptr crawlGait = std::make_unique(); + std::unique_ptr walkGait = std::make_unique(); + + MOTION_STATE motionState = MOTION_STATE::DEACTIVATED; unsigned long _lastUpdate; - constexpr static int MotionInterval = 100; + constexpr static int MotionInterval = 15; - body_state_t body_state = { - 0, - }; - float new_angles[12] = { - 0, - }; + body_state_t body_state = {0, 0, 0, 0, 0, 0}; + float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1}; - float default_feet_positions[4][4] = { - {100, -100, 100, 1}, {100, -100, -100, 1}, {-100, -100, 100, 1}, {-100, -100, -100, 1}}; + float default_feet_positions[4][4] = {{1, -1, 1, 1}, {1, -1, -1, 1}, {-1, -1, 1, 1}, {-1, -1, -1, 1}}; - float angles[12] = { - 0, - }; + float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145}; + float calibration_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; }; #endif diff --git a/esp32/lib/ESP32-sveltekit/Peripherals.h b/esp32/lib/ESP32-sveltekit/Peripherals.h index 92db4a5..0e49106 100644 --- a/esp32/lib/ESP32-sveltekit/Peripherals.h +++ b/esp32/lib/ESP32-sveltekit/Peripherals.h @@ -8,8 +8,8 @@ #include #include #include -#include +#include #include #include @@ -27,9 +27,10 @@ #define EVENT_I2C_SCAN "i2cScan" -#define I2C_INTERVAL 500 +#define I2C_INTERVAL 5000 #define MAX_ESP_IMU_SIZE 500 #define EVENT_IMU "imu" +#define EVENT_SERVO_STATE "servoState" /* * Servo Settings @@ -116,6 +117,7 @@ class Peripherals : public StatefulService { #endif _fsPersistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this, fs, DEVICE_CONFIG_FILE) { + _accessMutex = xSemaphoreCreateMutex(); addUpdateHandler([&](const String &originId) { updatePins(); }, false); }; @@ -140,6 +142,11 @@ class Peripherals : public StatefulService { _pca.begin(); _pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY); _pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY); + _pca.sleep(); + _socket->onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { + _pca_active = root["active"] | false; + _pca_active ? pcaActivate() : pcaDeactivate(); + }); #endif #if FT_ENABLED(FT_IMU) @@ -182,9 +189,11 @@ class Peripherals : public StatefulService { void loop() { EXECUTE_EVERY_N_MS(_updateInterval, { + beginTransaction(); updateImu(); readSonar(); emitSonar(); + endTransaction(); }); } @@ -231,16 +240,53 @@ class Peripherals : public StatefulService { } /* SERVO FUNCTIONS*/ - void pcaWrite(int index, int value) { + void pcaLerpTo(int index, int value, float t) { #if FT_ENABLED(FT_SERVO) - _pca.setPWM(index, 0, value); + target_pwm[index] = lerp(pwm[index], value, t); #endif } - void pcaAngle(int index, int value) { + void pcaWrite(int index, int value) { #if FT_ENABLED(FT_SERVO) - // uint16_t pwm = SERVO_MIN + value * SERVO_ANGLE_PWM_RATIO; - _pca.setPWM(index, 0, 125 + value * 2); + if (value < 0 || value > 4096) { + ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index); + return; + } + pwm[index] = value; + target_pwm[index] = value; + beginTransaction(); + _pca.setPWM(index, 0, value); + endTransaction(); +#endif + } + + void pcaWriteAngle(int index, int angle) { +#if FT_ENABLED(FT_SERVO) + _pca.setPWM(index, 0, 125 + angle * 2); +#endif + } + + void pcaWriteAngles(float *angles, int numServos, int offset = 0) { +#if FT_ENABLED(FT_SERVO) + for (int i = 0; i < numServos; i++) { + pcaWriteAngle(i + offset, angles[i]); + } +#endif + } + + void pcaActivate() { +#if FT_ENABLED(FT_SERVO) + if (_pca_active) return; + _pca_active = true; + _pca.wakeup(); +#endif + } + + void pcaDeactivate() { +#if FT_ENABLED(FT_SERVO) + if (!_pca_active) return; + _pca_active = false; + _pca.sleep(); #endif } @@ -405,11 +451,19 @@ class Peripherals : public StatefulService { EventEndpoint _eventEndpoint; FSPersistence _fsPersistence; + SemaphoreHandle_t _accessMutex; + inline void beginTransaction() { xSemaphoreTakeRecursive(_accessMutex, portMAX_DELAY); } + + inline void endTransaction() { xSemaphoreGiveRecursive(_accessMutex); } + JsonDocument doc; char message[MAX_ESP_IMU_SIZE]; #if FT_ENABLED(FT_SERVO) Adafruit_PWMServoDriver _pca; + bool _pca_active {false}; + uint16_t pwm[16] = {0}; + uint16_t target_pwm[16] = {0}; #endif #if FT_ENABLED(FT_IMU) MPU6050 _imu;