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>
61 lines
2.2 KiB
C++
61 lines
2.2 KiB
C++
#include "InputMapper.h"
|
|
#include <Arduino.h>
|
|
|
|
static float normalize(int16_t raw) { return constrain(raw, -1000, 1000) / 1000.0f; }
|
|
|
|
static float wrapDeg(float d) {
|
|
while (d > 180) d -= 360;
|
|
while (d < -180) d += 360;
|
|
return d;
|
|
}
|
|
|
|
MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
|
|
(void)dt;
|
|
MapperOutput out{};
|
|
out.armed = p.flags & FLAG_ARMED;
|
|
out.stand_request = p.flags & FLAG_STAND;
|
|
out.walk_active = p.flags & FLAG_WALK;
|
|
out.mode_b = p.flags & FLAG_MODE_B;
|
|
out.gait_wave = p.flags & FLAG_GAIT_WAVE;
|
|
out.remote_gyro_enabled = p.flags & FLAG_REMOTE_GYRO;
|
|
out.robot_gyro_enabled = p.flags & FLAG_ROBOT_GYRO;
|
|
|
|
float speed = p.speed_scale / 255.0f;
|
|
float raw_vx = normalize(p.vx) * speed;
|
|
float raw_vy = normalize(p.vy) * speed;
|
|
float raw_yaw_in = normalize(p.yaw_rate);
|
|
|
|
if (!out.mode_b) {
|
|
// Mode A: direct twist. Heading and travel direction are fully independent.
|
|
out.twist.vx = raw_vx * MAX_LINEAR_MM_S;
|
|
out.twist.vy = raw_vy * MAX_LINEAR_MM_S;
|
|
out.twist.omega = radians(raw_yaw_in * MAX_TURN_RATE_DEG_S);
|
|
} else {
|
|
// Mode B: heading-hold. Stick angle becomes a target heading; forward
|
|
// speed comes from stick magnitude. yaw_rate is unused here.
|
|
float mag = sqrt(raw_vx * raw_vx + raw_vy * raw_vy);
|
|
if (mag > 0.05f) {
|
|
heading_target_deg_ = degrees(atan2(raw_vy, raw_vx));
|
|
}
|
|
float heading_error = wrapDeg(heading_target_deg_ - heading_deg);
|
|
float omega_deg_s = constrain(HEADING_KP * heading_error, -MAX_TURN_RATE_DEG_S, MAX_TURN_RATE_DEG_S);
|
|
out.twist.omega = radians(omega_deg_s);
|
|
out.twist.vx = mag * MAX_LINEAR_MM_S; // forward-only in the robot's current facing
|
|
out.twist.vy = 0;
|
|
}
|
|
|
|
// Right stick: rotation sub-mode (default) or translation sub-mode.
|
|
float rx = normalize(p.rx), ry = normalize(p.ry), rz = normalize(p.rz);
|
|
if (p.flags & FLAG_RSTICK_XLATE) {
|
|
out.pose.x_mm = rx * MAX_POSE_OFFSET_MM;
|
|
out.pose.y_mm = ry * MAX_POSE_OFFSET_MM;
|
|
out.pose.z_mm = rz * MAX_POSE_OFFSET_MM;
|
|
} else {
|
|
out.pose.roll_deg = rx * MAX_TILT_DEG;
|
|
out.pose.pitch_deg = ry * MAX_TILT_DEG;
|
|
out.pose.yaw_deg = rz * MAX_TILT_DEG;
|
|
}
|
|
|
|
return out;
|
|
}
|