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.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
app/src/lib/gait.ts
+9 -10
View File
@@ -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],
esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+11 -14
View File
@@ -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<float>(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); }
esp32/lib/ESP32-sveltekit/MotionService.h
+5 -4
View File
@@ -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};