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

🐕 Makes gaits speed variable

Browse Source
This commit is contained in:
Rune Harlyk
2024-08-03 16:02:03 +02:00
committed by Rune Harlyk
parent e532ae7929
commit 5b6f27d692
4 changed files with 31 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+6 -2
View File
@@ -43,6 +43,8 @@
[ModesEnum.Crawl]: new EightPhaseWalkState(), [ModesEnum.Crawl]: new EightPhaseWalkState(),
[ModesEnum.Walk]: new FourPhaseWalkState() [ModesEnum.Walk]: new FourPhaseWalkState()
} }
let lastTick = performance.now()
const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1] const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
let body_state = { let body_state = {
@@ -244,11 +246,13 @@
h: controlData[5], h: controlData[5],
s: controlData[6], 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)] 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.omega = body_state.omega
settings.phi = body_state.phi settings.phi = body_state.phi
app/src/lib/gait.ts
+9 -10
View File
@@ -52,10 +52,10 @@ export abstract class GaitState {
map_command(command: ControllerCommand): gait_state_t { map_command(command: ControllerCommand): gait_state_t {
return { 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_x: (Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10) * 3,
step_z: -(Math.floor(fromInt8(command.lx, -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_angle: 0,
step_depth: 0.2 step_depth: 0.2
}; };
@@ -100,7 +100,7 @@ abstract class PhaseGaitState extends GaitState {
protected phase = 0; protected phase = 0;
protected phase_time = 0; protected phase_time = 0;
protected abstract num_phases: number; protected abstract num_phases: number;
protected abstract phase_length: number; protected abstract phase_speed_factor: number;
protected abstract swing_stand_ratio: number; protected abstract swing_stand_ratio: number;
protected contact_phases!: 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) { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
this.body_state = body_state; this.body_state = body_state;
this.gait_state = this.map_command(command); this.gait_state = this.map_command(command);
this.dt = dt; this.dt = dt / 1000;
this.update_phase(); this.update_phase();
this.update_body_position(); this.update_body_position();
this.update_feet_positions(); this.update_feet_positions();
@@ -121,13 +121,12 @@ abstract class PhaseGaitState extends GaitState {
} }
update_phase() { update_phase() {
this.tick += 1; this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity;
this.phase_time = this.tick / this.phase_length;
if (this.tick % this.phase_length == 0) { if (this.phase_time >= 1) {
this.phase += 1; this.phase += 1;
if (this.phase == this.num_phases) this.phase = 0; 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 { export class FourPhaseWalkState extends PhaseGaitState {
protected name = 'Four phase walk'; protected name = 'Four phase walk';
protected num_phases = 4; protected num_phases = 4;
protected phase_length = 15; protected phase_speed_factor = 2.5;
protected contact_phases = [ protected contact_phases = [
[1, 0, 1, 1], [1, 0, 1, 1],
[1, 1, 1, 0], [1, 1, 1, 0],
@@ -214,7 +213,7 @@ export class FourPhaseWalkState extends PhaseGaitState {
export class EightPhaseWalkState extends PhaseGaitState { export class EightPhaseWalkState extends PhaseGaitState {
protected name = 'Eight phase walk'; protected name = 'Eight phase walk';
protected num_phases = 8; protected num_phases = 8;
protected phase_length = 20; protected phase_speed_factor = 1.5;
protected contact_phases = [ protected contact_phases = [
[1, 0, 1, 1, 1, 1, 1, 1], [1, 0, 1, 1, 1, 1, 1, 1],
[1, 1, 1, 0, 1, 1, 1, 1], [1, 1, 1, 0, 1, 1, 1, 1],
esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+11 -14
View File
@@ -16,7 +16,7 @@ struct ControllerCommand {
class GaitState { class GaitState {
protected: protected:
virtual const char *name() const = 0; 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: public:
virtual float getDefaultHeight() const { return 0.5f; } virtual float getDefaultHeight() const { return 0.5f; }
@@ -64,11 +64,10 @@ class StandState : public GaitState {
class PhaseGaitState : public GaitState { class PhaseGaitState : public GaitState {
protected: protected:
int tick = 0;
int phase = 0; int phase = 0;
float phase_time = 0; float phase_time = 0;
virtual int num_phases() const = 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; virtual float swing_stand_ratio() const = 0;
uint8_t contact_phases[4][8]; 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 { void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
this->gait_state = mapCommand(command); this->gait_state = mapCommand(command);
this->dt = dt;
updatePhase(); updatePhase();
updateBodyPosition(body_state); updateBodyPosition(body_state);
updateFeetPositions(body_state); updateFeetPositions(body_state);
} }
void updatePhase() { void updatePhase() {
tick += 1; phase_time += dt * phase_speed_factor() * gait_state.step_velocity;
phase_time = static_cast<float>(tick) / phase_length();
if (tick % phase_length() == 0) { if (phase_time >= 1.0f) {
phase += 1; phase += 1;
if (phase == num_phases()) phase = 0; 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 mapCommand(ControllerCommand command) {
gait_state_t state; gait_state_t state;
state.step_height = 0.4f; state.step_height = 0.4f + std::fabs(command.ry / 128);
state.step_x = command.ly / 128 * 3; state.step_x = command.ly / 128;
state.step_z = -command.lx / 128 * 3; state.step_z = -command.lx / 128;
state.step_velocity = 1; state.step_velocity = command.s / 128 + 1;
state.step_angle = 0; state.step_angle = 0;
return state; return state;
} }
@@ -158,7 +155,7 @@ class FourPhaseWalkState : public PhaseGaitState {
int num_phases() const override { return 4; } 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); } 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 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); } float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); }
esp32/lib/ESP32-sveltekit/MotionService.h
+5 -4
View File
@@ -73,14 +73,15 @@ class MotionService {
command.h = array[5]; command.h = array[5];
command.s = array[6]; 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) { switch (motionState) {
case MOTION_STATE::STAND: { case MOTION_STATE::STAND: {
body_state.phi = command.rx / 4; body_state.phi = command.rx / 8;
body_state.psi = command.ry / 4; body_state.psi = command.ry / 8;
body_state.xm = command.ly / 2 / 100; body_state.xm = command.ly / 2 / 100;
body_state.zm = command.lx / 2 / 100; body_state.zm = command.lx / 2 / 100;
body_state.updateFeet(default_feet_positions);
break; break;
} }
} }
@@ -168,7 +169,7 @@ class MotionService {
float new_angles[12] = {0, 0, 0, 0, 0, 0, 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 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 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 rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};