Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🎉 Connects motion service with servo controller

Browse Source
This commit is contained in:
Rune Harlyk
2024-08-03 14:03:57 +02:00
committed by Rune Harlyk
parent 4e75952f57
commit e532ae7929
5 changed files with 134 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
+1 -4
View File
@@ -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
esp32/lib/ESP32-sveltekit/Kinematics.h
+8 -7
View File
@@ -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);
esp32/lib/ESP32-sveltekit/MotionService.h
+61 -41
View File
@@ -4,6 +4,7 @@
#include <EventSocket.h>
#include <TaskManager.h>
#include <Kinematics.h>
#include <ServoController.h>
#include <Gait/GaitState.h>
#include <Timing.h>
#include <MathUtils.h>
@@ -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<JsonArray>();
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<int>());
motionState = (MOTION_STATE)root["data"].as<int>();
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<MotionService *>(_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<GaitState> crawlGait = std::make_unique<EightPhaseWalkState>();
std::unique_ptr<GaitState> walkGait = std::make_unique<FourPhaseWalkState>();
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
esp32/lib/ESP32-sveltekit/Peripherals.h
+61 -7
View File
@@ -8,8 +8,8 @@
#include <StatefulService.h>
#include <MathUtils.h>
#include <Timing.h>
#include <list>
#include <list>
#include <SPI.h>
#include <Wire.h>
@@ -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<PeripheralsConfiguration> {
#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<PeripheralsConfiguration> {
_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<PeripheralsConfiguration> {
void loop() {
EXECUTE_EVERY_N_MS(_updateInterval, {
beginTransaction();
updateImu();
readSonar();
emitSonar();
endTransaction();
});
}
@@ -231,16 +240,53 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
}
/* 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<PeripheralsConfiguration> {
EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
FSPersistence<PeripheralsConfiguration> _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;