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
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};