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>
70 lines
2.8 KiB
C++
70 lines
2.8 KiB
C++
#include "PacketBuilder.h"
|
|
|
|
// Handset tilt (deg) -> -1000..1000 with a small deadzone for hand tremor,
|
|
// scaled by the fade blend so toggling remote gyro ramps instead of steps.
|
|
int16_t PacketBuilder::tiltToWire(float deg, float blend) {
|
|
float mag = fabsf(deg) - REMOTE_TILT_DEADZONE_DEG;
|
|
if (mag <= 0 || blend <= 0) return 0;
|
|
float n = constrain(mag / (REMOTE_TILT_FULL_SCALE_DEG - REMOTE_TILT_DEADZONE_DEG), 0.0f, 1.0f);
|
|
return (int16_t)lroundf(copysignf(n, deg) * 1000.0f * blend);
|
|
}
|
|
|
|
CmdPacket PacketBuilder::build(const StickFrame& s, const RemoteIMU& imu, const RemoteState& st, uint32_t now_ms) {
|
|
CmdPacket p;
|
|
p.seq = seq_++;
|
|
p.speed_scale = st.speedCurrent();
|
|
|
|
uint8_t f = 0;
|
|
if (st.armed()) f |= FLAG_ARMED;
|
|
if (st.standRequested()) f |= FLAG_STAND;
|
|
if (st.modeB()) f |= FLAG_MODE_B;
|
|
if (st.rstickXlate()) f |= FLAG_RSTICK_XLATE;
|
|
if (st.gaitWave()) f |= FLAG_GAIT_WAVE;
|
|
if (st.robotGyro()) f |= FLAG_ROBOT_GYRO;
|
|
|
|
// Disarmed: send a neutral command. The robot ignores motion then anyway,
|
|
// but this keeps its pose smoothers from winding up to a stale stick.
|
|
if (!st.armed()) {
|
|
p.flags = f;
|
|
return p;
|
|
}
|
|
|
|
// ---- Left stick: walking. Handset stick frame -> body frame:
|
|
// stick forward (+Y) -> +vx forward, stick right (+X) -> -vy (vy + = left),
|
|
// twist CCW -> +yaw_rate (turn left).
|
|
p.vx = s.v[L_Y];
|
|
p.vy = -s.v[L_X];
|
|
p.yaw_rate = s.v[L_TWIST];
|
|
|
|
// ---- Right stick: body pose. Each packet field is "about / along" one body
|
|
// axis, so the physical-axis mapping differs per sub-mode:
|
|
if (st.rstickXlate()) {
|
|
p.rx = s.v[R_Y]; // X offset: stick forward -> body forward
|
|
p.ry = -s.v[R_X]; // Y offset: stick right -> body right (-Y)
|
|
p.rz = s.v[R_TWIST]; // Z offset: twist CCW -> body up
|
|
} else {
|
|
p.rx = s.v[R_X]; // roll: stick right -> right side down
|
|
p.ry = s.v[R_Y]; // pitch: stick forward -> nose down
|
|
p.rz = s.v[R_TWIST]; // yaw: twist CCW -> body yaws left
|
|
}
|
|
|
|
// ---- Walk flag: in mode B the robot ignores yaw_rate, so twist alone
|
|
// shouldn't start stepping. Linger avoids STAND<->WALKING chatter.
|
|
bool walk_input = p.vx != 0 || p.vy != 0 || (!st.modeB() && p.yaw_rate != 0);
|
|
if (walk_input) { walk_seen_ = true; last_walk_input_ms_ = now_ms; }
|
|
bool walking = walk_seen_ && (now_ms - last_walk_input_ms_) < WALK_LINGER_MS;
|
|
if (st.standRequested() && (walk_input || walking)) f |= FLAG_WALK;
|
|
|
|
// ---- Remote tilt feedforward: keep the flag up for the whole fade-out so
|
|
// the robot sees the tilt ramp to zero rather than being cut off.
|
|
float blend = st.remoteTiltBlend();
|
|
if (st.remoteGyro() || blend > 0.0f) {
|
|
f |= FLAG_REMOTE_GYRO;
|
|
p.remote_roll = tiltToWire(imu.rollDeg(), blend);
|
|
p.remote_pitch = tiltToWire(imu.pitchDeg(), blend);
|
|
}
|
|
|
|
p.flags = f;
|
|
return p;
|
|
}
|