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

♻️ Moves motion implementation to source file

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-11 17:26:09 +02:00
committed by Rune Harlyk
parent 7fd35f3f48
commit 41b863a0eb
3 changed files with 100 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/motion.h
+13 -103
View File
@@ -1,9 +1,11 @@
#ifndef MotionService_h
#define MotionService_h
#include <event_socket.h>
#include <ArduinoJson.h>
#include <kinematics.h>
#include <peripherals/servo_controller.h>
#include <peripherals/gesture.h>
#include <peripherals/peripherals.h>
#include <utils/timing.h>
#include <utils/math_utils.h>
@@ -17,121 +19,29 @@ enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
class MotionService {
public:
MotionService() {}
void begin();
void begin() { body_state.updateFeet(KinConfig::default_feet_positions); }
void anglesEvent(JsonVariant &root, int originId);
void anglesEvent(JsonVariant &root, int originId) {
JsonArray array = root.as<JsonArray>();
for (int i = 0; i < 12; i++) {
angles[i] = array[i];
}
syncAngles(String(originId));
}
void handleInput(JsonVariant &root, int originId);
void setState(MotionState *newState) {
if (state) {
state->end();
}
state = newState;
if (state) {
_servoController->activate();
state->begin();
}
}
void handleWalkGait(JsonVariant &root, int originId);
void handleInput(JsonVariant &root, int originId) {
command.fromJson(root);
if (state) state->handleCommand(command);
}
void handleMode(JsonVariant &root, int originId);
void handleWalkGait(JsonVariant &root, int originId) {
ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
void setState(MotionState *newState);
WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
if (walkGait == WALK_GAIT::TROT)
walkState.set_mode_trot();
else
walkState.set_mode_crawl();
}
void handleGestures(const gesture_t ges);
void handleMode(JsonVariant &root, int originId) {
MOTION_STATE mode = static_cast<MOTION_STATE>(root.as<int>());
ESP_LOGV("MotionService", "Mode %d", mode);
switch (mode) {
case MOTION_STATE::REST: setState(&restState); break;
case MOTION_STATE::STAND: setState(&standState); break;
case MOTION_STATE::WALK: setState(&walkState); break;
case MOTION_STATE::DEACTIVATED:
setState(nullptr);
_servoController->deactivate();
break;
default: setState(nullptr); break;
}
JsonDocument doc;
doc.set(static_cast<int>(mode));
JsonVariant data = doc.as<JsonVariant>();
// socket.emit(MODE_EVENT, data, String(originId).c_str());
}
bool update(Peripherals *peripherals);
void emitAngles(const String &originId = "", bool sync = false) {
JsonDocument doc;
auto arr = doc.to<JsonArray>();
for (int i = 0; i < 12; i++) arr.add(angles[i]);
JsonVariant data = doc.as<JsonVariant>();
// socket.emit(ANGLES_EVENT, data, originId.c_str());
}
void syncAngles(const String &originId = "", bool sync = false) {
emitAngles(originId, sync);
_servoController->setAngles(angles);
}
void handleGestures(const gesture_t ges) {
if (ges != gesture_t::eGestureNone) {
ESP_LOGI("Motion", "Gesture: %d", ges);
switch (ges) {
case gesture_t::eGestureDown: setState(&restState); break;
case gesture_t::eGestureUp: setState(&standState); break;
case gesture_t::eGestureLeft:
case gesture_t::eGestureRight: setState(&walkState); break;
default: break;
}
}
}
bool update(Peripherals *peripherals) {
handleGestures(peripherals->takeGesture());
if (!state) return false;
unsigned long now = millis();
float dt = (now - lastUpdate) / 1000.0f;
lastUpdate = now;
state->updateImuOffsets(peripherals->angleY(), peripherals->angleX());
state->step(body_state, dt);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
return update_angles(new_angles, angles);
}
bool update_angles(float new_angles[12], float angles[12]) {
bool updated = false;
for (int i = 0; i < 12; i++) {
const float new_angle = new_angles[i] * dir[i];
if (!isEqual(new_angle, angles[i], 0.1)) {
angles[i] = new_angle;
updated = true;
}
}
return updated;
}
bool update_angles(float new_angles[12], float angles[12]);
float *getAngles() { return angles; }
inline bool isActive() { return state != nullptr; }
private:
ServoController *_servoController;
Kinematics kinematics;
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
esp32/src/main.cpp
+1 -3
View File
@@ -85,9 +85,6 @@ void setupEventSocket() {
[&](JsonVariant &root, int originId) { motionService.handleWalkGait(root, originId); });
socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { motionService.anglesEvent(root, originId); });
socket.onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, motionService, std::placeholders::_1,
std::placeholders::_2));
}
void IRAM_ATTR SpotControlLoopEntry(void *) {
@@ -115,6 +112,7 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
void IRAM_ATTR serviceLoopEntry(void *) {
ESP_LOGI("main", "Service control task starting");
wifiService.begin();
MDNS.begin(APP_NAME);
MDNS.setInstanceName(APP_NAME);
esp32/src/motion.cpp
+86
View File
@@ -0,0 +1,86 @@
#include <motion.h>
void MotionService::begin() { body_state.updateFeet(KinConfig::default_feet_positions); }
void MotionService::anglesEvent(JsonVariant &root, int originId) {
JsonArray array = root.as<JsonArray>();
for (int i = 0; i < 12; i++) {
angles[i] = array[i];
}
}
void MotionService::setState(MotionState *newState) {
if (state) {
state->end();
}
state = newState;
if (state) {
state->begin();
}
}
void MotionService::handleInput(JsonVariant &root, int originId) {
command.fromJson(root);
if (state) state->handleCommand(command);
}
void MotionService::handleWalkGait(JsonVariant &root, int originId) {
ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
if (walkGait == WALK_GAIT::TROT)
walkState.set_mode_trot();
else
walkState.set_mode_crawl();
}
void MotionService::handleMode(JsonVariant &root, int originId) {
MOTION_STATE mode = static_cast<MOTION_STATE>(root.as<int>());
ESP_LOGV("MotionService", "Mode %d", mode);
switch (mode) {
case MOTION_STATE::REST: setState(&restState); break;
case MOTION_STATE::STAND: setState(&standState); break;
case MOTION_STATE::WALK: setState(&walkState); break;
case MOTION_STATE::DEACTIVATED: setState(nullptr); break;
default: setState(nullptr); break;
}
}
void MotionService::handleGestures(const gesture_t ges) {
if (ges != gesture_t::eGestureNone) {
ESP_LOGI("Motion", "Gesture: %d", ges);
switch (ges) {
case gesture_t::eGestureDown: setState(&restState); break;
case gesture_t::eGestureUp: setState(&standState); break;
case gesture_t::eGestureLeft:
case gesture_t::eGestureRight: setState(&walkState); break;
default: break;
}
}
}
bool MotionService::update(Peripherals *peripherals) {
handleGestures(peripherals->takeGesture());
if (!state) return false;
unsigned long now = millis();
float dt = (now - lastUpdate) / 1000.0f;
lastUpdate = now;
state->updateImuOffsets(peripherals->angleY(), peripherals->angleX());
state->step(body_state, dt);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
return update_angles(new_angles, angles);
}
bool MotionService::update_angles(float new_angles[12], float angles[12]) {
bool updated = false;
for (int i = 0; i < 12; i++) {
const float new_angle = new_angles[i] * dir[i];
if (!isEqual(new_angle, angles[i], 0.1)) {
angles[i] = new_angle;
updated = true;
}
}
return updated;
}