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

🪄 Formats MotionService

Browse Source
This commit is contained in:
Rune Harlyk
2024-07-09 20:08:57 +02:00
committed by Rune Harlyk
parent 9ea6eb2b5d
commit 1aba163b60
1 changed files with 30 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/MotionService.h
+30 -48
View File
@@ -12,24 +12,15 @@
#define POSITION_EVENT "position" #define POSITION_EVENT "position"
#define MODE_EVENT "mode" #define MODE_EVENT "mode"
enum class MOTION_STATE enum class MOTION_STATE { IDLE, REST, STAND, WALK };
{
IDLE,
REST,
STAND,
WALK
};
class MotionService class MotionService {
{
public: public:
MotionService(PsychicHttpServer *server, EventSocket *socket, SecurityManager *securityManager, TaskManager *taskManager) MotionService(PsychicHttpServer *server, EventSocket *socket, SecurityManager *securityManager,
: _server(server), _socket(socket), _securityManager(securityManager), _taskManager(taskManager) TaskManager *taskManager)
{ : _server(server), _socket(socket), _securityManager(securityManager), _taskManager(taskManager) {}
}
void begin() void begin() {
{
_socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); }); _socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); });
_socket->onEvent(MODE_EVENT, [&](JsonObject &root, int originId) { handleMode(root, originId); }); _socket->onEvent(MODE_EVENT, [&](JsonObject &root, int originId) { handleMode(root, originId); });
@@ -38,23 +29,21 @@ class MotionService
_socket->onEvent(POSITION_EVENT, [&](JsonObject &root, int originId) { positionEvent(root, originId); }); _socket->onEvent(POSITION_EVENT, [&](JsonObject &root, int originId) { positionEvent(root, originId); });
_socket->onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2)); _socket->onSubscribe(ANGLES_EVENT,
std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
body_state.updateFeet(default_feet_positions); body_state.updateFeet(default_feet_positions);
} }
void anglesEvent(JsonObject &root, int originId) void anglesEvent(JsonObject &root, int originId) {
{
JsonArray array = root["data"].as<JsonArray>(); JsonArray array = root["data"].as<JsonArray>();
for (int i = 0; i < 12; i++) for (int i = 0; i < 12; i++) {
{
angles[i] = array[i]; angles[i] = array[i];
} }
syncAngles(String(originId)); syncAngles(String(originId));
} }
void positionEvent(JsonObject &root, int originId) void positionEvent(JsonObject &root, int originId) {
{
JsonArray array = root["data"].as<JsonArray>(); JsonArray array = root["data"].as<JsonArray>();
body_state.omega = array[0]; body_state.omega = array[0];
body_state.phi = array[1]; body_state.phi = array[1];
@@ -64,8 +53,7 @@ class MotionService
body_state.zm = array[5]; body_state.zm = array[5];
} }
void handleInput(JsonObject &root, int originId) void handleInput(JsonObject &root, int originId) {
{
JsonArray array = root["data"].as<JsonArray>(); JsonArray array = root["data"].as<JsonArray>();
float lx = array[1]; float lx = array[1];
float ly = array[2]; float ly = array[2];
@@ -87,8 +75,7 @@ class MotionService
} }
} }
void handleMode(JsonObject &root, int originId) void handleMode(JsonObject &root, int originId) {
{
ESP_LOGV("MotionService", "Mode %d", root["data"].as<int>()); ESP_LOGV("MotionService", "Mode %d", root["data"].as<int>());
motionState = (MOTION_STATE)root["data"].as<int>(); motionState = (MOTION_STATE)root["data"].as<int>();
char output[2]; char output[2];
@@ -98,32 +85,25 @@ class MotionService
void syncAngles(const String &originId = "", bool sync = false) { void syncAngles(const String &originId = "", bool sync = false) {
char output[100]; 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], sprintf(output, "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0], angles[1],
angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]); 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()); _socket->emit(ANGLES_EVENT, output, originId.c_str());
} }
float lerp(float start, float end, float t) { float lerp(float start, float end, float t) { return (1 - t) * start + t * end; }
return (1 - t) * start + t * end;
}
bool updateMotion() { bool updateMotion() {
switch (motionState) { switch (motionState) {
case MOTION_STATE::IDLE: case MOTION_STATE::IDLE: return false; break;
return false;
break;
case MOTION_STATE::REST: case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break;
update_angles(rest_angles, new_angles, false);
break;
case MOTION_STATE::STAND: { case MOTION_STATE::STAND: {
kinematics.calculate_inverse_kinematics(body_state, new_angles); kinematics.calculate_inverse_kinematics(body_state, new_angles);
break; break;
} }
case MOTION_STATE::WALK: case MOTION_STATE::WALK: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
} }
return update_angles(new_angles, angles); return update_angles(new_angles, angles);
} }
@@ -158,18 +138,20 @@ class MotionService
unsigned long _lastUpdate; unsigned long _lastUpdate;
constexpr static int MotionInterval = 100; constexpr static int MotionInterval = 100;
body_state_t body_state = {0,}; body_state_t body_state = {
float new_angles[12] = {0,}; 0,
};
float new_angles[12] = {
0,
};
float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1}; float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
float default_feet_positions[4][4] = { float default_feet_positions[4][4] = {
{ 100, -100, 100, 1}, {100, -100, 100, 1}, {100, -100, -100, 1}, {-100, -100, 100, 1}, {-100, -100, -100, 1}};
{ 100, -100, -100, 1},
{-100, -100, 100, 1},
{-100, -100, -100, 1}
};
float angles[12] = {0,}; float angles[12] = {
0,
};
float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145}; float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
}; };