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

🥽 Makes use of Atan2 to calculate lateral fraction

Browse Source
This commit is contained in:
Rune Harlyk
2024-11-09 14:14:47 +01:00
committed by Rune Harlyk
parent 4de6be7815
commit 634b3292b4
2 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/gait.ts
+5 -6
View File
@@ -289,7 +289,6 @@ export class BezierState extends GaitState {
this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2); this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2);
if (this.gait_state.step_x < 0) { if (this.gait_state.step_x < 0) {
this.step_length = -this.step_length; this.step_length = -this.step_length;
this.gait_state.step_z = -this.gait_state.step_z;
} }
this.update_phase(); this.update_phase();
this.update_feet_positions(); this.update_feet_positions();
@@ -335,7 +334,7 @@ export class BezierState extends GaitState {
...args: number[] ...args: number[]
) { ) {
let length = this.step_length / 2; let length = this.step_length / 2;
let angle = (this.gait_state.step_z * Math.PI) / 2; let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2;
const delta_pos = controller(length, angle, ...args, this.phase); const delta_pos = controller(length, angle, ...args, this.phase);
length = this.gait_state.step_angle * 2; length = this.gait_state.step_angle * 2;
@@ -358,13 +357,13 @@ const stance_curve = (length: number, angle: number, depth: number, phase: numbe
const step = length * (1 - 2 * phase); const step = length * (1 - 2 * phase);
const X = step * X_POLAR; const X = step * X_POLAR;
const Y = step * Y_POLAR; const Z = step * Y_POLAR;
let Z = 0; let Y = 0;
if (length !== 0) { if (length !== 0) {
Z = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length)); Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length));
} }
return [X, Z, Y]; return [X, Y, Z];
}; };
const yawArc = (default_foot_pos: number[], current_foot_pos: number[]): number => { const yawArc = (default_foot_pos: number[], current_foot_pos: number[]): number => {
esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+1 -2
View File
@@ -220,7 +220,6 @@ class BezierState : public GaitState {
step_length = std::sqrt(gait_state.step_x * gait_state.step_x + gait_state.step_z * gait_state.step_z); step_length = std::sqrt(gait_state.step_x * gait_state.step_x + gait_state.step_z * gait_state.step_z);
if (gait_state.step_x < 0.0f) { if (gait_state.step_x < 0.0f) {
step_length = -step_length; step_length = -step_length;
gait_state.step_z = -gait_state.step_z;
} }
updatePhase(dt); updatePhase(dt);
updateFeetPositions(body_state); updateFeetPositions(body_state);
@@ -261,7 +260,7 @@ class BezierState : public GaitState {
void controller(const int index, body_state_t &body_state, void controller(const int index, body_state_t &body_state,
std::function<void(float, float, float *, float, float *)> curve, float *arg) { std::function<void(float, float, float *, float, float *)> curve, float *arg) {
float length = step_length / 2.0f; float length = step_length / 2.0f;
float angle = gait_state.step_z * M_PI_2; float angle = std::atan2(gait_state.step_z, step_length) * 2;
float point[3] = {0, 0, 0}; float point[3] = {0, 0, 0};
curve(length, angle, arg, phase_time, point); curve(length, angle, arg, phase_time, point);