From 5b6f27d69262c5f60fe5840201420b01da2de6ad Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Sat, 3 Aug 2024 16:02:03 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=95=20Makes=20gaits=20speed=20variable?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/components/Visualization.svelte | 8 +++++-- app/src/lib/gait.ts | 19 ++++++++-------- esp32/lib/ESP32-sveltekit/Gait/GaitState.h | 25 +++++++++------------ esp32/lib/ESP32-sveltekit/MotionService.h | 9 ++++---- 4 files changed, 31 insertions(+), 30 deletions(-) diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index 4c3082d..2b5e291 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -43,6 +43,8 @@ [ModesEnum.Crawl]: new EightPhaseWalkState(), [ModesEnum.Walk]: new FourPhaseWalkState() } + let lastTick = performance.now() + const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1] let body_state = { @@ -244,11 +246,13 @@ h: controlData[5], s: controlData[6], }; - body_state.ym = ((data.h + 128) * 0.75) / 100; + body_state.ym = ((data.h + 127) * 0.75) / 100; let planner = planners[get(mode)] + const delta = performance.now() - lastTick + lastTick = performance.now() - body_state = planner.step(body_state, data); + body_state = planner.step(body_state, data, delta); settings.omega = body_state.omega settings.phi = body_state.phi diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index 0cfe99d..7c7ab18 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -52,10 +52,10 @@ export abstract class GaitState { map_command(command: ControllerCommand): gait_state_t { return { - step_height: 0.4, + step_height: 0.4 + Math.abs(command.ry / 128), step_x: (Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10) * 3, step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10) * 3, - step_velocity: 1, + step_velocity: command.s / 128 + 1, step_angle: 0, step_depth: 0.2 }; @@ -100,7 +100,7 @@ abstract class PhaseGaitState extends GaitState { protected phase = 0; protected phase_time = 0; protected abstract num_phases: number; - protected abstract phase_length: number; + protected abstract phase_speed_factor: number; protected abstract swing_stand_ratio: number; protected contact_phases!: number[][]; @@ -113,7 +113,7 @@ abstract class PhaseGaitState extends GaitState { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { this.body_state = body_state; this.gait_state = this.map_command(command); - this.dt = dt; + this.dt = dt / 1000; this.update_phase(); this.update_body_position(); this.update_feet_positions(); @@ -121,13 +121,12 @@ abstract class PhaseGaitState extends GaitState { } update_phase() { - this.tick += 1; - this.phase_time = this.tick / this.phase_length; + this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity; - if (this.tick % this.phase_length == 0) { + if (this.phase_time >= 1) { this.phase += 1; if (this.phase == this.num_phases) this.phase = 0; - this.tick = 0; + this.phase_time = 0; } } @@ -189,7 +188,7 @@ abstract class PhaseGaitState extends GaitState { export class FourPhaseWalkState extends PhaseGaitState { protected name = 'Four phase walk'; protected num_phases = 4; - protected phase_length = 15; + protected phase_speed_factor = 2.5; protected contact_phases = [ [1, 0, 1, 1], [1, 1, 1, 0], @@ -214,7 +213,7 @@ export class FourPhaseWalkState extends PhaseGaitState { export class EightPhaseWalkState extends PhaseGaitState { protected name = 'Eight phase walk'; protected num_phases = 8; - protected phase_length = 20; + protected phase_speed_factor = 1.5; protected contact_phases = [ [1, 0, 1, 1, 1, 1, 1, 1], [1, 1, 1, 0, 1, 1, 1, 1], diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h index 7c0ee49..e82864b 100644 --- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h +++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h @@ -16,7 +16,7 @@ struct ControllerCommand { class GaitState { protected: virtual const char *name() const = 0; - float default_feet_positions[4][4] = {{1, -1, 1, 1}, {1, -1, -1, 1}, {-1, -1, 1, 1}, {-1, -1, -1, 1}}; + float default_feet_positions[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}; public: virtual float getDefaultHeight() const { return 0.5f; } @@ -64,11 +64,10 @@ class StandState : public GaitState { class PhaseGaitState : public GaitState { protected: - int tick = 0; int phase = 0; float phase_time = 0; virtual int num_phases() const = 0; - virtual int phase_length() const = 0; + virtual float phase_speed_factor() const = 0; virtual float swing_stand_ratio() const = 0; uint8_t contact_phases[4][8]; @@ -79,20 +78,18 @@ class PhaseGaitState : public GaitState { void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override { this->gait_state = mapCommand(command); - this->dt = dt; updatePhase(); updateBodyPosition(body_state); updateFeetPositions(body_state); } void updatePhase() { - tick += 1; - phase_time = static_cast(tick) / phase_length(); + phase_time += dt * phase_speed_factor() * gait_state.step_velocity; - if (tick % phase_length() == 0) { + if (phase_time >= 1.0f) { phase += 1; if (phase == num_phases()) phase = 0; - tick = 0; + phase_time = 0; } } @@ -143,10 +140,10 @@ class PhaseGaitState : public GaitState { gait_state_t mapCommand(ControllerCommand command) { gait_state_t state; - state.step_height = 0.4f; - state.step_x = command.ly / 128 * 3; - state.step_z = -command.lx / 128 * 3; - state.step_velocity = 1; + state.step_height = 0.4f + std::fabs(command.ry / 128); + state.step_x = command.ly / 128; + state.step_z = -command.lx / 128; + state.step_velocity = command.s / 128 + 1; state.step_angle = 0; return state; } @@ -158,7 +155,7 @@ class FourPhaseWalkState : public PhaseGaitState { int num_phases() const override { return 4; } - int phase_length() const override { return 15; } + float phase_speed_factor() const override { return 2.5; } float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); } @@ -183,7 +180,7 @@ class EightPhaseWalkState : public PhaseGaitState { int num_phases() const override { return 8; } - int phase_length() const override { return 20; } + float phase_speed_factor() const override { return 1.5; } float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); } diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h index 38d276a..c13d0f6 100644 --- a/esp32/lib/ESP32-sveltekit/MotionService.h +++ b/esp32/lib/ESP32-sveltekit/MotionService.h @@ -73,14 +73,15 @@ class MotionService { command.h = array[5]; command.s = array[6]; - body_state.ym = (command.h + 128.f) * 0.75f / 100; + body_state.ym = (command.h + 127.f) * 0.75f / 100; switch (motionState) { case MOTION_STATE::STAND: { - body_state.phi = command.rx / 4; - body_state.psi = command.ry / 4; + body_state.phi = command.rx / 8; + body_state.psi = command.ry / 8; body_state.xm = command.ly / 2 / 100; body_state.zm = command.lx / 2 / 100; + body_state.updateFeet(default_feet_positions); break; } } @@ -168,7 +169,7 @@ class MotionService { 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] = {{1, -1, 1, 1}, {1, -1, -1, 1}, {-1, -1, 1, 1}, {-1, -1, -1, 1}}; + float default_feet_positions[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}; 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};