✨ Adds skill system
This commit is contained in:
Rune Harlyk
@@ -0,0 +1,156 @@
|
||||
<script lang="ts">
|
||||
import { skill } from '$lib/stores'
|
||||
import { onMount, onDestroy } from 'svelte'
|
||||
|
||||
let targetX = $state(0.5)
|
||||
let targetZ = $state(0)
|
||||
let targetYaw = $state(0)
|
||||
let speed = $state(0.5)
|
||||
|
||||
const status = skill.status
|
||||
const isActive = skill.isActive
|
||||
const progress = skill.progress
|
||||
|
||||
const presets = [
|
||||
{ name: 'Forward 0.5m', x: 0.5, z: 0, yaw: 0 },
|
||||
{ name: 'Forward 1m', x: 1, z: 0, yaw: 0 },
|
||||
{ name: 'Back 0.5m', x: -0.5, z: 0, yaw: 0 },
|
||||
{ name: 'Left 0.5m', x: 0, z: 0.5, yaw: 0 },
|
||||
{ name: 'Right 0.5m', x: 0, z: -0.5, yaw: 0 },
|
||||
{ name: 'Turn Left 90°', x: 0, z: 0, yaw: 1.57 },
|
||||
{ name: 'Turn Right 90°', x: 0, z: 0, yaw: -1.57 },
|
||||
{ name: 'Turn 180°', x: 0, z: 0, yaw: 3.14 }
|
||||
]
|
||||
|
||||
onMount(() => skill.init())
|
||||
onDestroy(() => skill.destroy())
|
||||
|
||||
function executeSkill() {
|
||||
skill.walk(targetX, targetZ, targetYaw, speed)
|
||||
}
|
||||
|
||||
function runPreset(preset: (typeof presets)[0]) {
|
||||
skill.walk(preset.x, preset.z, preset.yaw, speed)
|
||||
}
|
||||
|
||||
function formatMeters(val: number): string {
|
||||
return val.toFixed(3) + 'm'
|
||||
}
|
||||
|
||||
function formatDegrees(rad: number): string {
|
||||
return ((rad * 180) / Math.PI).toFixed(1) + '°'
|
||||
}
|
||||
</script>
|
||||
|
||||
<div class="card bg-base-200 shadow-xl">
|
||||
<div class="card-body p-4">
|
||||
<h2 class="card-title text-sm flex justify-between">
|
||||
Skill Control
|
||||
<span class="badge" class:badge-success={$isActive} class:badge-ghost={!$isActive}>
|
||||
{$isActive ? 'Active' : 'Idle'}
|
||||
</span>
|
||||
</h2>
|
||||
|
||||
<div class="grid grid-cols-2 gap-2 text-xs mb-2">
|
||||
<div class="stat bg-base-300 rounded-lg p-2">
|
||||
<div class="stat-title text-xs">Position</div>
|
||||
<div class="stat-value text-sm">
|
||||
{formatMeters($status.x)}, {formatMeters($status.z)}
|
||||
</div>
|
||||
<div class="stat-desc">Yaw: {formatDegrees($status.yaw)}</div>
|
||||
</div>
|
||||
<div class="stat bg-base-300 rounded-lg p-2">
|
||||
<div class="stat-title text-xs">Distance</div>
|
||||
<div class="stat-value text-sm">{formatMeters($status.distance)}</div>
|
||||
<div class="stat-desc">Total traveled</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{#if $isActive}
|
||||
<div class="mb-2">
|
||||
<div class="flex justify-between text-xs mb-1">
|
||||
<span>Progress</span>
|
||||
<span>{($progress * 100).toFixed(0)}%</span>
|
||||
</div>
|
||||
<progress class="progress progress-primary w-full" value={$progress} max="1"></progress>
|
||||
<div class="text-xs text-base-content/60 mt-1">
|
||||
Target: ({$status.skill_target_x.toFixed(2)}, {$status.skill_target_z.toFixed(2)}, {formatDegrees(
|
||||
$status.skill_target_yaw
|
||||
)})
|
||||
</div>
|
||||
</div>
|
||||
{/if}
|
||||
|
||||
<div class="divider my-1 text-xs">Presets</div>
|
||||
|
||||
<div class="grid grid-cols-4 gap-1">
|
||||
{#each presets as preset}
|
||||
<button class="btn btn-xs btn-outline" onclick={() => runPreset(preset)}>
|
||||
{preset.name}
|
||||
</button>
|
||||
{/each}
|
||||
</div>
|
||||
|
||||
<div class="divider my-1 text-xs">Custom</div>
|
||||
|
||||
<div class="grid grid-cols-3 gap-2">
|
||||
<div class="form-control">
|
||||
<label class="label py-0" for="skill-x">
|
||||
<span class="label-text text-xs">X (m)</span>
|
||||
</label>
|
||||
<input
|
||||
id="skill-x"
|
||||
type="number"
|
||||
step="0.1"
|
||||
bind:value={targetX}
|
||||
class="input input-bordered input-xs w-full"
|
||||
/>
|
||||
</div>
|
||||
<div class="form-control">
|
||||
<label class="label py-0" for="skill-z">
|
||||
<span class="label-text text-xs">Z (m)</span>
|
||||
</label>
|
||||
<input
|
||||
id="skill-z"
|
||||
type="number"
|
||||
step="0.1"
|
||||
bind:value={targetZ}
|
||||
class="input input-bordered input-xs w-full"
|
||||
/>
|
||||
</div>
|
||||
<div class="form-control">
|
||||
<label class="label py-0" for="skill-yaw">
|
||||
<span class="label-text text-xs">Yaw (rad)</span>
|
||||
</label>
|
||||
<input
|
||||
id="skill-yaw"
|
||||
type="number"
|
||||
step="0.1"
|
||||
bind:value={targetYaw}
|
||||
class="input input-bordered input-xs w-full"
|
||||
/>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="form-control mt-2">
|
||||
<label class="label py-0" for="skill-speed">
|
||||
<span class="label-text text-xs">Speed: {speed.toFixed(2)}</span>
|
||||
</label>
|
||||
<input id="skill-speed" type="range" min="0.1" max="1" step="0.05" bind:value={speed} class="range range-xs range-primary" />
|
||||
</div>
|
||||
|
||||
<div class="card-actions justify-between mt-2">
|
||||
<div class="flex gap-1">
|
||||
<button class="btn btn-xs btn-ghost" onclick={() => skill.resetPosition()}>Reset Pos</button>
|
||||
</div>
|
||||
<div class="flex gap-1">
|
||||
<button class="btn btn-xs btn-error" onclick={() => skill.stop()} disabled={!$isActive}>
|
||||
Stop
|
||||
</button>
|
||||
<button class="btn btn-xs btn-primary" onclick={executeSkill} disabled={$isActive}>
|
||||
Execute
|
||||
</button>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
@@ -4,6 +4,7 @@ import { get, type Writable } from 'svelte/store'
|
||||
import Visualization from '$lib/components/Visualization.svelte'
|
||||
import Stream from '$lib/components/Stream.svelte'
|
||||
import ChartWidget from '$lib/components/widget/ChartWidget.svelte'
|
||||
import SkillPanel from '$lib/components/SkillPanel.svelte'
|
||||
|
||||
export interface WidgetConfig {
|
||||
id: string | number
|
||||
@@ -25,7 +26,8 @@ export const isWidgetConfig = (
|
||||
export const WidgetComponents = {
|
||||
Visualization,
|
||||
Stream,
|
||||
ChartWidget
|
||||
ChartWidget,
|
||||
SkillPanel
|
||||
}
|
||||
|
||||
interface View {
|
||||
@@ -59,6 +61,16 @@ const defaultViews: View[] = [
|
||||
{ id: 2, component: 'Visualization', props: { debug: true } }
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
name: 'Skills',
|
||||
content: {
|
||||
id: 'root',
|
||||
widgets: [
|
||||
{ id: 1, component: 'Visualization', props: { debug: true } },
|
||||
{ id: 2, component: 'SkillPanel' }
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
|
||||
@@ -7,3 +7,4 @@ export * from './telemetry'
|
||||
export * from './analytics'
|
||||
export * from './featureFlags'
|
||||
export * from './location-store'
|
||||
export * from './skill'
|
||||
|
||||
@@ -0,0 +1,85 @@
|
||||
import { writable, derived } from 'svelte/store'
|
||||
import { socket } from './socket'
|
||||
import { MessageTopic, type SkillStatus, type SkillCommand } from '$lib/types/models'
|
||||
|
||||
const defaultStatus: SkillStatus = {
|
||||
x: 0,
|
||||
y: 0,
|
||||
z: 0,
|
||||
yaw: 0,
|
||||
distance: 0,
|
||||
skill_active: false,
|
||||
skill_target_x: 0,
|
||||
skill_target_z: 0,
|
||||
skill_target_yaw: 0,
|
||||
skill_traveled_x: 0,
|
||||
skill_traveled_z: 0,
|
||||
skill_rotated: 0,
|
||||
skill_progress: 0,
|
||||
skill_complete: false
|
||||
}
|
||||
|
||||
function createSkillStore() {
|
||||
const status = writable<SkillStatus>(defaultStatus)
|
||||
const history = writable<SkillCommand[]>([])
|
||||
let unsubscribe: (() => void) | null = null
|
||||
|
||||
function init() {
|
||||
if (unsubscribe) return
|
||||
unsubscribe = socket.on<SkillStatus>(MessageTopic.skillStatus, data => {
|
||||
status.set(data)
|
||||
if (data.event === 'complete') {
|
||||
history.update(h => [...h.slice(-9), getCurrentTarget(data)])
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
function getCurrentTarget(s: SkillStatus): SkillCommand {
|
||||
return { x: s.skill_target_x, z: s.skill_target_z, yaw: s.skill_target_yaw }
|
||||
}
|
||||
|
||||
function execute(cmd: SkillCommand) {
|
||||
socket.sendEvent(MessageTopic.skill, cmd)
|
||||
}
|
||||
|
||||
function walk(x: number, z: number = 0, yaw: number = 0, speed: number = 0.5) {
|
||||
execute({ x, z, yaw, speed })
|
||||
}
|
||||
|
||||
function turn(yaw: number, speed: number = 0.5) {
|
||||
execute({ x: 0, z: 0, yaw, speed })
|
||||
}
|
||||
|
||||
function stop() {
|
||||
socket.sendEvent(MessageTopic.displacement, { action: 'clear' })
|
||||
}
|
||||
|
||||
function resetPosition() {
|
||||
socket.sendEvent(MessageTopic.displacement, { action: 'reset' })
|
||||
}
|
||||
|
||||
function destroy() {
|
||||
if (unsubscribe) {
|
||||
unsubscribe()
|
||||
unsubscribe = null
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
status,
|
||||
history,
|
||||
init,
|
||||
destroy,
|
||||
execute,
|
||||
walk,
|
||||
turn,
|
||||
stop,
|
||||
resetPosition,
|
||||
isActive: derived(status, $s => $s.skill_active),
|
||||
progress: derived(status, $s => $s.skill_progress),
|
||||
position: derived(status, $s => ({ x: $s.x, y: $s.y, z: $s.z, yaw: $s.yaw }))
|
||||
}
|
||||
}
|
||||
|
||||
export const skill = createSkillStore()
|
||||
|
||||
@@ -14,7 +14,10 @@ export enum MessageTopic {
|
||||
servoPWM = 'servoPWM',
|
||||
WiFiSettings = 'WiFiSettings',
|
||||
sonar = 'sonar',
|
||||
rssi = 'rssi'
|
||||
rssi = 'rssi',
|
||||
skill = 'skill',
|
||||
skillStatus = 'skill_status',
|
||||
displacement = 'displacement'
|
||||
}
|
||||
|
||||
export type vector = { x: number; y: number }
|
||||
@@ -248,3 +251,28 @@ export interface MDNSStatus {
|
||||
services: MDNSService[]
|
||||
global_txt_records: MDNSTxtRecord[]
|
||||
}
|
||||
|
||||
export interface SkillCommand {
|
||||
x: number
|
||||
z: number
|
||||
yaw: number
|
||||
speed?: number
|
||||
}
|
||||
|
||||
export interface SkillStatus {
|
||||
x: number
|
||||
y: number
|
||||
z: number
|
||||
yaw: number
|
||||
distance: number
|
||||
skill_active: boolean
|
||||
skill_target_x: number
|
||||
skill_target_z: number
|
||||
skill_target_yaw: number
|
||||
skill_traveled_x: number
|
||||
skill_traveled_z: number
|
||||
skill_rotated: number
|
||||
skill_progress: number
|
||||
skill_complete: boolean
|
||||
event?: string
|
||||
}
|
||||
|
||||
@@ -38,7 +38,6 @@ class KinConfig {
|
||||
{mountOffsets[3][0], 0, mountOffsets[3][2] - coxa, 1},
|
||||
};
|
||||
|
||||
// Max constants
|
||||
static constexpr float max_roll = 15 * DEG2RAD_F;
|
||||
static constexpr float max_pitch = 15 * DEG2RAD_F;
|
||||
|
||||
@@ -60,12 +59,85 @@ class KinConfig {
|
||||
static constexpr float default_step_height = default_body_height / 2;
|
||||
};
|
||||
|
||||
struct displacement_state_t {
|
||||
float x {0};
|
||||
float y {0};
|
||||
float z {0};
|
||||
float roll {0};
|
||||
float pitch {0};
|
||||
float yaw {0};
|
||||
|
||||
void reset() { x = y = z = roll = pitch = yaw = 0; }
|
||||
float distance() const { return std::sqrt(x * x + z * z); }
|
||||
};
|
||||
|
||||
struct skill_target_t {
|
||||
float target_x {0};
|
||||
float target_z {0};
|
||||
float target_yaw {0};
|
||||
|
||||
float traveled_x {0};
|
||||
float traveled_z {0};
|
||||
float rotated {0};
|
||||
|
||||
bool active {false};
|
||||
|
||||
void set(float x, float z, float yaw) {
|
||||
target_x = x;
|
||||
target_z = z;
|
||||
target_yaw = yaw;
|
||||
traveled_x = traveled_z = rotated = 0;
|
||||
active = true;
|
||||
}
|
||||
|
||||
void reset() {
|
||||
target_x = target_z = target_yaw = 0;
|
||||
traveled_x = traveled_z = rotated = 0;
|
||||
active = false;
|
||||
}
|
||||
|
||||
void accumulate(float dx, float dz, float dyaw) {
|
||||
traveled_x += dx;
|
||||
traveled_z += dz;
|
||||
rotated += dyaw;
|
||||
}
|
||||
|
||||
bool isComplete() const {
|
||||
if (!active) return false;
|
||||
bool x_ok = (target_x == 0) || (target_x > 0 ? traveled_x >= target_x : traveled_x <= target_x);
|
||||
bool z_ok = (target_z == 0) || (target_z > 0 ? traveled_z >= target_z : traveled_z <= target_z);
|
||||
bool yaw_ok = (target_yaw == 0) || (target_yaw > 0 ? rotated >= target_yaw : rotated <= target_yaw);
|
||||
return x_ok && z_ok && yaw_ok;
|
||||
}
|
||||
|
||||
float progress() const {
|
||||
if (!active) return 0;
|
||||
float total_target = std::fabs(target_x) + std::fabs(target_z) + std::fabs(target_yaw);
|
||||
if (total_target == 0) return 1;
|
||||
|
||||
auto clampProgress = [](float traveled, float target) -> float {
|
||||
if (target == 0) return 0;
|
||||
float p = traveled / target;
|
||||
return std::clamp(p, 0.0f, 1.0f) * std::fabs(target);
|
||||
};
|
||||
|
||||
float total_progress = clampProgress(traveled_x, target_x) + clampProgress(traveled_z, target_z) +
|
||||
clampProgress(rotated, target_yaw);
|
||||
return total_progress / total_target;
|
||||
}
|
||||
};
|
||||
|
||||
struct alignas(16) body_state_t {
|
||||
float omega {0}, phi {0}, psi {0}, xm {0}, ym {KinConfig::default_body_height}, zm {0};
|
||||
float feet[4][4];
|
||||
|
||||
displacement_state_t cumulative;
|
||||
skill_target_t skill;
|
||||
|
||||
void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); }
|
||||
|
||||
void resetDisplacement() { cumulative.reset(); }
|
||||
|
||||
bool operator==(const body_state_t &other) const {
|
||||
if (!IS_ALMOST_EQUAL(omega, other.omega) || !IS_ALMOST_EQUAL(phi, other.phi) ||
|
||||
!IS_ALMOST_EQUAL(psi, other.psi) || !IS_ALMOST_EQUAL(xm, other.xm) || !IS_ALMOST_EQUAL(ym, other.ym) ||
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
|
||||
#include <ArduinoJson.h>
|
||||
#include "esp_timer.h"
|
||||
#include <functional>
|
||||
|
||||
#include <kinematics.h>
|
||||
#include <peripherals/gesture.h>
|
||||
@@ -18,6 +19,8 @@
|
||||
|
||||
enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
|
||||
|
||||
using SkillCompleteCallback = std::function<void()>;
|
||||
|
||||
class MotionService {
|
||||
public:
|
||||
void begin();
|
||||
@@ -30,6 +33,10 @@ class MotionService {
|
||||
|
||||
void handleMode(JsonVariant &root, int originId);
|
||||
|
||||
void handleDisplacement(JsonVariant &root, int originId);
|
||||
|
||||
void handleSkill(JsonVariant &root, int originId);
|
||||
|
||||
void setState(MotionState *newState);
|
||||
|
||||
void handleGestures(const gesture_t ges);
|
||||
@@ -42,6 +49,39 @@ class MotionService {
|
||||
|
||||
inline bool isActive() { return state != nullptr; }
|
||||
|
||||
void resetDisplacement() { body_state.resetDisplacement(); }
|
||||
|
||||
void setSkillTarget(float x, float z, float yaw) { body_state.skill.set(x, z, yaw); }
|
||||
|
||||
void clearSkill() { body_state.skill.reset(); }
|
||||
|
||||
bool isSkillActive() const { return body_state.skill.active; }
|
||||
|
||||
bool isSkillComplete() const { return body_state.skill.isComplete(); }
|
||||
|
||||
const displacement_state_t &getDisplacement() const { return body_state.cumulative; }
|
||||
|
||||
const skill_target_t &getSkill() const { return body_state.skill; }
|
||||
|
||||
void getDisplacementResult(JsonVariant &root) const {
|
||||
root["x"] = body_state.cumulative.x;
|
||||
root["y"] = body_state.cumulative.y;
|
||||
root["z"] = body_state.cumulative.z;
|
||||
root["yaw"] = body_state.cumulative.yaw;
|
||||
root["distance"] = body_state.cumulative.distance();
|
||||
root["skill_active"] = body_state.skill.active;
|
||||
root["skill_target_x"] = body_state.skill.target_x;
|
||||
root["skill_target_z"] = body_state.skill.target_z;
|
||||
root["skill_target_yaw"] = body_state.skill.target_yaw;
|
||||
root["skill_traveled_x"] = body_state.skill.traveled_x;
|
||||
root["skill_traveled_z"] = body_state.skill.traveled_z;
|
||||
root["skill_rotated"] = body_state.skill.rotated;
|
||||
root["skill_progress"] = body_state.skill.progress();
|
||||
root["skill_complete"] = body_state.skill.isComplete();
|
||||
}
|
||||
|
||||
void setSkillCompleteCallback(SkillCompleteCallback callback) { skillCompleteCallback = callback; }
|
||||
|
||||
private:
|
||||
Kinematics kinematics;
|
||||
|
||||
@@ -63,6 +103,11 @@ class MotionService {
|
||||
float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
|
||||
|
||||
int64_t lastUpdate = esp_timer_get_time();
|
||||
|
||||
SkillCompleteCallback skillCompleteCallback = nullptr;
|
||||
bool skillWasComplete = false;
|
||||
|
||||
void checkSkillComplete();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -98,11 +98,33 @@ class WalkState : public MotionState {
|
||||
|
||||
step_length = std::hypot(gait_state.step_x, gait_state.step_z);
|
||||
if (gait_state.step_x < 0.0f) step_length = -step_length;
|
||||
|
||||
const bool moving = !isZero(gait_state.step_x) || !isZero(gait_state.step_z) || !isZero(gait_state.step_angle);
|
||||
updateDisplacement(body_state, dt, moving);
|
||||
|
||||
updatePhase(dt);
|
||||
updateBodyPosition(body_state, dt);
|
||||
updateFeetPositions(body_state);
|
||||
}
|
||||
|
||||
void updateDisplacement(body_state_t &body_state, float dt, bool moving) {
|
||||
if (!moving) return;
|
||||
|
||||
float dx_local = gait_state.step_x * gait_state.step_velocity * dt * speed_factor;
|
||||
float dz_local = gait_state.step_z * gait_state.step_velocity * dt * speed_factor;
|
||||
float dyaw = gait_state.step_angle * gait_state.step_velocity * dt * speed_factor;
|
||||
|
||||
if (body_state.skill.active) {
|
||||
body_state.skill.accumulate(dx_local, dz_local, dyaw);
|
||||
}
|
||||
|
||||
float cos_yaw = std::cos(body_state.cumulative.yaw);
|
||||
float sin_yaw = std::sin(body_state.cumulative.yaw);
|
||||
body_state.cumulative.x += dx_local * cos_yaw - dz_local * sin_yaw;
|
||||
body_state.cumulative.z += dx_local * sin_yaw + dz_local * cos_yaw;
|
||||
body_state.cumulative.yaw += dyaw;
|
||||
}
|
||||
|
||||
protected:
|
||||
void handleCommand(const CommandMsg &cmd) override {
|
||||
target_body_state.ym = KinConfig::min_body_height + cmd.h * KinConfig::body_height_range;
|
||||
|
||||
@@ -144,6 +144,9 @@ void setupServer() {
|
||||
#define EVENT_I2C_SCAN "i2cScan"
|
||||
#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
|
||||
#define EVENT_SERVO_STATE "servoState"
|
||||
#define EVENT_DISPLACEMENT "displacement"
|
||||
#define EVENT_SKILL "skill"
|
||||
#define EVENT_SKILL_STATUS "skill_status"
|
||||
#define EVENT_IMU_CALIBRATE "imuCalibrate"
|
||||
|
||||
void setupEventSocket() {
|
||||
@@ -182,6 +185,27 @@ void setupEventSocket() {
|
||||
[&](JsonVariant &root, int originId) { servoController.servoEvent(root, originId); });
|
||||
socket.onEvent(EVENT_SERVO_STATE,
|
||||
[&](JsonVariant &root, int originId) { servoController.stateUpdate(root, originId); });
|
||||
|
||||
// Skill events
|
||||
socket.onEvent(EVENT_DISPLACEMENT,
|
||||
[&](JsonVariant &root, int originId) { motionService.handleDisplacement(root, originId); });
|
||||
|
||||
socket.onEvent(EVENT_SKILL, [&](JsonVariant &root, int originId) { motionService.handleSkill(root, originId); });
|
||||
|
||||
socket.onEvent(EVENT_SKILL_STATUS, [&](JsonVariant &root, int originId) {
|
||||
JsonDocument doc;
|
||||
JsonVariant results = doc.to<JsonVariant>();
|
||||
motionService.getDisplacementResult(results);
|
||||
socket.emit(EVENT_SKILL_STATUS, results);
|
||||
});
|
||||
|
||||
motionService.setSkillCompleteCallback([&]() {
|
||||
JsonDocument doc;
|
||||
JsonVariant results = doc.to<JsonVariant>();
|
||||
motionService.getDisplacementResult(results);
|
||||
results["event"] = "complete";
|
||||
socket.emit(EVENT_SKILL_STATUS, results);
|
||||
});
|
||||
}
|
||||
|
||||
void IRAM_ATTR SpotControlLoopEntry(void *) {
|
||||
@@ -235,6 +259,14 @@ void IRAM_ATTR serviceLoopEntry(void *) {
|
||||
peripherals.getIMUResult(results);
|
||||
socket.emit(EVENT_IMU, results);
|
||||
});
|
||||
EXECUTE_EVERY_N_MS(200, {
|
||||
if (motionService.isSkillActive()) {
|
||||
JsonDocument doc;
|
||||
JsonVariant results = doc.to<JsonVariant>();
|
||||
motionService.getDisplacementResult(results);
|
||||
socket.emit(EVENT_SKILL_STATUS, results);
|
||||
}
|
||||
});
|
||||
|
||||
vTaskDelay(100 / portTICK_PERIOD_MS);
|
||||
}
|
||||
|
||||
+62
-1
@@ -46,6 +46,65 @@ void MotionService::handleMode(JsonVariant &root, int originId) {
|
||||
}
|
||||
}
|
||||
|
||||
void MotionService::handleDisplacement(JsonVariant &root, int originId) {
|
||||
std::string action = root["action"] | "";
|
||||
if (action == "reset") {
|
||||
resetDisplacement();
|
||||
ESP_LOGI("MotionService", "Displacement reset");
|
||||
} else if (action == "clear") {
|
||||
clearSkill();
|
||||
skillWasComplete = false;
|
||||
ESP_LOGI("MotionService", "Skill cleared");
|
||||
}
|
||||
}
|
||||
|
||||
void MotionService::handleSkill(JsonVariant &root, int originId) {
|
||||
float x = root["x"] | 0.0f;
|
||||
float z = root["z"] | 0.0f;
|
||||
float yaw = root["yaw"] | 0.0f;
|
||||
float speed = root["speed"] | 0.5f;
|
||||
|
||||
setSkillTarget(x, z, yaw);
|
||||
skillWasComplete = false;
|
||||
|
||||
float linear_mag = std::hypot(x, z);
|
||||
bool has_linear = linear_mag > 0.001f;
|
||||
bool has_yaw = std::fabs(yaw) > 0.001f;
|
||||
|
||||
if (has_linear || has_yaw) {
|
||||
if (has_linear) {
|
||||
float norm_x = x / linear_mag;
|
||||
float norm_z = z / linear_mag;
|
||||
command.ly = norm_x;
|
||||
command.lx = -norm_z;
|
||||
} else {
|
||||
command.ly = 0;
|
||||
command.lx = 0;
|
||||
}
|
||||
command.rx = has_yaw ? (yaw > 0 ? 1.0f : -1.0f) : 0;
|
||||
command.s = speed;
|
||||
if (state) state->handleCommand(command);
|
||||
}
|
||||
|
||||
ESP_LOGI("MotionService", "Skill set: Walk(%.3f, %.3f, %.3f) speed=%.2f", x, z, yaw, speed);
|
||||
}
|
||||
|
||||
void MotionService::checkSkillComplete() {
|
||||
if (!body_state.skill.active) return;
|
||||
if (skillWasComplete) return;
|
||||
|
||||
if (body_state.skill.isComplete()) {
|
||||
skillWasComplete = true;
|
||||
command = {0, 0, 0, 0, command.h, 0, command.s1};
|
||||
if (state) state->handleCommand(command);
|
||||
ESP_LOGI("MotionService", "Skill complete! Traveled: (%.3f, %.3f), Rotated: %.3f", body_state.skill.traveled_x,
|
||||
body_state.skill.traveled_z, body_state.skill.rotated);
|
||||
if (skillCompleteCallback) {
|
||||
skillCompleteCallback();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MotionService::handleGestures(const gesture_t ges) {
|
||||
if (ges != gesture_t::eGestureNone) {
|
||||
ESP_LOGI("Motion", "Gesture: %d", ges);
|
||||
@@ -64,10 +123,12 @@ bool MotionService::update(Peripherals *peripherals) {
|
||||
handleGestures(peripherals->takeGesture());
|
||||
if (!state) return false;
|
||||
int64_t now = esp_timer_get_time();
|
||||
float dt = (now - lastUpdate) / 1000000.0f; // Convert microseconds to seconds
|
||||
float dt = (now - lastUpdate) / 1000000.0f;
|
||||
lastUpdate = now;
|
||||
state->updateImuOffsets(peripherals->angleY(), peripherals->angleX());
|
||||
ESP_LOGI("MotionService", "IMU Offsets: %.3f, %.3f", peripherals->angleY(), peripherals->angleX());
|
||||
state->step(body_state, dt);
|
||||
checkSkillComplete();
|
||||
kinematics.calculate_inverse_kinematics(body_state, new_angles);
|
||||
|
||||
return update_angles(new_angles, angles);
|
||||
|
||||
Reference in New Issue
Block a user