mangavalkandClaude Opus 5.5
91dac16fca
Not yet verified on hardware. - shared/HexapodProtocol: CmdPacket (now 20 bytes, adds remote_roll / remote_pitch), FLAG_* bits and radio settings as the single protocol definition; static payload size = sizeof(CmdPacket) on both radios. - robot: use the shared protocol; vx = forward (harness w/x <-> q/e); InputMapper adds remote tilt on top of the right stick when FLAG_REMOTE_GYRO is set, and robot.ino no longer zeroes stick roll/pitch when it's off. - remote (Teensy 4.1): Sticks (calibration, EEPROM), RemoteIMU, RemoteState (interlocks, link-loss disarm, smoothing), PacketBuilder, RadioLink, NextionUI, SerialConsole. - CLAUDE.md, README.md, remote/README.md, remote/NEXTION.md. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
71 lines
2.8 KiB
C++
71 lines
2.8 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;
|
|
}
|
|
|
|
// Remote handset tilt feedforward: its own packet fields, gated only by
|
|
// FLAG_REMOTE_GYRO, added on top of whatever the right stick commands (so
|
|
// it works in both right-stick sub-modes). Total tilt stays within MAX_TILT_DEG.
|
|
if (out.remote_gyro_enabled) {
|
|
out.pose.roll_deg = constrain(out.pose.roll_deg + normalize(p.remote_roll) * MAX_TILT_DEG,
|
|
-MAX_TILT_DEG, MAX_TILT_DEG);
|
|
out.pose.pitch_deg = constrain(out.pose.pitch_deg + normalize(p.remote_pitch) * MAX_TILT_DEG,
|
|
-MAX_TILT_DEG, MAX_TILT_DEG);
|
|
}
|
|
|
|
return out;
|
|
}
|