mangavalkandClaude Opus 5.5
ad77b06c36
Robot-side firmware (Teensy 4.0) imported unchanged as extracted. platformio.ini defines [env:robot] (teensy40) and [env:remote] (teensy41); scripts/select_src_dir.py points each env at its own sketch folder so robot/ and remote/ stay Arduino-style sketches. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
36 lines
1.1 KiB
C++
36 lines
1.1 KiB
C++
#include "Limits.h"
|
|
#include <Arduino.h>
|
|
#include <math.h>
|
|
|
|
FootTarget Limits::clampWorkspace(int i, const FootTarget& target) {
|
|
FootTarget neutral = Kinematics::neutralStance(i);
|
|
float dx = target.x - neutral.x;
|
|
float dy = target.y - neutral.y;
|
|
float dz = target.z - neutral.z;
|
|
|
|
constexpr float MAX_HORIZ_MM = 70.0f; // tune to your real leg reach, with margin
|
|
constexpr float MAX_VERT_MM = 60.0f;
|
|
|
|
float horiz = sqrt(dx * dx + dy * dy);
|
|
if (horiz > MAX_HORIZ_MM) {
|
|
float scale = MAX_HORIZ_MM / horiz;
|
|
dx *= scale;
|
|
dy *= scale;
|
|
}
|
|
dz = constrain(dz, -MAX_VERT_MM, MAX_VERT_MM);
|
|
|
|
return { neutral.x + dx, neutral.y + dy, neutral.z + dz };
|
|
}
|
|
|
|
float Limits::clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt) {
|
|
float max_step = max_deg_per_sec * dt;
|
|
float delta = constrain(target_deg - current_deg, -max_step, max_step);
|
|
return current_deg + delta;
|
|
}
|
|
|
|
bool Limits::stancePolygonOk(const bool in_stance[6]) {
|
|
int count = 0;
|
|
for (int i = 0; i < 6; i++) if (in_stance[i]) count++;
|
|
return count >= 3;
|
|
}
|