From ad77b06c3611b320c677f95f8f45029d3c5f46c7 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Mon, 28 Sep 2026 15:36:37 +0200 Subject: [PATCH] Initial import: robot firmware + PlatformIO two-env scaffold 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 --- .gitattributes | 37 ++++++ 2026-current/.gitignore | 28 +++++ 2026-current/Hexapod.code-workspace | 7 ++ 2026-current/platformio.ini | 29 +++++ 2026-current/robot/BodyPose.cpp | 50 ++++++++ 2026-current/robot/BodyPose.h | 36 ++++++ 2026-current/robot/Comms.cpp | 28 +++++ 2026-current/robot/Comms.h | 40 ++++++ 2026-current/robot/Config.h | 59 +++++++++ 2026-current/robot/GaitEngine.cpp | 65 ++++++++++ 2026-current/robot/GaitEngine.h | 35 ++++++ 2026-current/robot/InputMapper.cpp | 60 +++++++++ 2026-current/robot/InputMapper.h | 30 +++++ 2026-current/robot/Kinematics.cpp | 67 ++++++++++ 2026-current/robot/Kinematics.h | 22 ++++ 2026-current/robot/Limits.cpp | 35 ++++++ 2026-current/robot/Limits.h | 15 +++ 2026-current/robot/README.md | 113 +++++++++++++++++ 2026-current/robot/RobotIMU.cpp | 51 ++++++++ 2026-current/robot/RobotIMU.h | 18 +++ 2026-current/robot/ServoDriver.cpp | 28 +++++ 2026-current/robot/ServoDriver.h | 9 ++ 2026-current/robot/SmoothedValue.h | 25 ++++ 2026-current/robot/StateMachine.cpp | 22 ++++ 2026-current/robot/StateMachine.h | 17 +++ 2026-current/robot/robot.ino | 165 +++++++++++++++++++++++++ 2026-current/scripts/select_src_dir.py | 16 +++ 27 files changed, 1107 insertions(+) create mode 100644 .gitattributes create mode 100644 2026-current/.gitignore create mode 100644 2026-current/Hexapod.code-workspace create mode 100644 2026-current/platformio.ini create mode 100644 2026-current/robot/BodyPose.cpp create mode 100644 2026-current/robot/BodyPose.h create mode 100644 2026-current/robot/Comms.cpp create mode 100644 2026-current/robot/Comms.h create mode 100644 2026-current/robot/Config.h create mode 100644 2026-current/robot/GaitEngine.cpp create mode 100644 2026-current/robot/GaitEngine.h create mode 100644 2026-current/robot/InputMapper.cpp create mode 100644 2026-current/robot/InputMapper.h create mode 100644 2026-current/robot/Kinematics.cpp create mode 100644 2026-current/robot/Kinematics.h create mode 100644 2026-current/robot/Limits.cpp create mode 100644 2026-current/robot/Limits.h create mode 100644 2026-current/robot/README.md create mode 100644 2026-current/robot/RobotIMU.cpp create mode 100644 2026-current/robot/RobotIMU.h create mode 100644 2026-current/robot/ServoDriver.cpp create mode 100644 2026-current/robot/ServoDriver.h create mode 100644 2026-current/robot/SmoothedValue.h create mode 100644 2026-current/robot/StateMachine.cpp create mode 100644 2026-current/robot/StateMachine.h create mode 100644 2026-current/robot/robot.ino create mode 100644 2026-current/scripts/select_src_dir.py diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..533fe76 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,37 @@ +*.wmv filter=lfs diff=lfs merge=lfs -text +*.WMV filter=lfs diff=lfs merge=lfs -text +*.mp4 filter=lfs diff=lfs merge=lfs -text +*.MP4 filter=lfs diff=lfs merge=lfs -text +*.MPG filter=lfs diff=lfs merge=lfs -text +*.mpg filter=lfs diff=lfs merge=lfs -text +*.jpg filter=lfs diff=lfs merge=lfs -text +*.JPG filter=lfs diff=lfs merge=lfs -text +*.jpeg filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.PNG filter=lfs diff=lfs merge=lfs -text +*.bmp filter=lfs diff=lfs merge=lfs -text +*.exr filter=lfs diff=lfs merge=lfs -text +*.pdf filter=lfs diff=lfs merge=lfs -text +*.docx filter=lfs diff=lfs merge=lfs -text +*.vsdx filter=lfs diff=lfs merge=lfs -text +*.dwg filter=lfs diff=lfs merge=lfs -text +*.f3d filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text +*.zi filter=lfs diff=lfs merge=lfs -text +*.stl filter=lfs diff=lfs merge=lfs -text +*.STL filter=lfs diff=lfs merge=lfs -text +*.HMI filter=lfs diff=lfs merge=lfs -text +*.exe filter=lfs diff=lfs merge=lfs -text +*.dll filter=lfs diff=lfs merge=lfs -text +*.pdb filter=lfs diff=lfs merge=lfs -text +*.idb filter=lfs diff=lfs merge=lfs -text +*.obj filter=lfs diff=lfs merge=lfs -text +*.ipch filter=lfs diff=lfs merge=lfs -text +*.sdf filter=lfs diff=lfs merge=lfs -text +*.suo filter=lfs diff=lfs merge=lfs -text +*.db filter=lfs diff=lfs merge=lfs -text +*.lnk filter=lfs diff=lfs merge=lfs -text +*.ide filter=lfs diff=lfs merge=lfs -text +*.ide-wal filter=lfs diff=lfs merge=lfs -text +*.ide-shm filter=lfs diff=lfs merge=lfs -text +2018-unity-sim/**/*.unity filter=lfs diff=lfs merge=lfs -text diff --git a/2026-current/.gitignore b/2026-current/.gitignore new file mode 100644 index 0000000..3a2e7b9 --- /dev/null +++ b/2026-current/.gitignore @@ -0,0 +1,28 @@ +# PlatformIO +.pio/ +.pioenvs/ +.piolibdeps/ +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch/ + +# Arduino IDE / arduino-cli build output +build/ +*.elf +*.hex +*.bin +*.map +*.o +*.d + +# Nextion Editor build output (keep the .HMI source, ignore compiled .tft) +*.tft + +# OS / editor noise +.DS_Store +Thumbs.db +desktop.ini +*.swp +*~ +__pycache__/ diff --git a/2026-current/Hexapod.code-workspace b/2026-current/Hexapod.code-workspace new file mode 100644 index 0000000..362d7c2 --- /dev/null +++ b/2026-current/Hexapod.code-workspace @@ -0,0 +1,7 @@ +{ + "folders": [ + { + "path": "." + } + ] +} \ No newline at end of file diff --git a/2026-current/platformio.ini b/2026-current/platformio.ini new file mode 100644 index 0000000..34c3a41 --- /dev/null +++ b/2026-current/platformio.ini @@ -0,0 +1,29 @@ +; Hexapod: two firmware images in one project. +; pio run -e robot build robot/ (Teensy 4.0) +; pio run -e remote build remote/ (Teensy 4.1) +; pio run -e robot -t upload flash (Teensy Loader / teensy_cli) +; pio device monitor -e robot serial monitor at 115200 +; +; shared/ is the library directory. Both sketches #include +; from shared/HexapodProtocol/, which is the single definition of the wire +; protocol (CmdPacket, FLAG_* bits, radio address/channel/rate/PA/payload size). + +[platformio] +default_envs = robot, remote +lib_dir = shared + +[env] +platform = teensy +framework = arduino +monitor_speed = 115200 +extra_scripts = pre:scripts/select_src_dir.py +lib_deps = + nrf24/RF24@^1.4.9 + +[env:robot] +board = teensy40 +custom_src_dir = robot + +[env:remote] +board = teensy41 +custom_src_dir = remote diff --git a/2026-current/robot/BodyPose.cpp b/2026-current/robot/BodyPose.cpp new file mode 100644 index 0000000..b46e49a --- /dev/null +++ b/2026-current/robot/BodyPose.cpp @@ -0,0 +1,50 @@ +#include "BodyPose.h" + +void BodyPose::begin() { + s_roll_.reset(0); s_pitch_.reset(0); s_yaw_.reset(0); + s_x_.reset(0); s_y_.reset(0); s_z_.reset(0); +} + +void BodyPose::update(float dt) { + s_roll_.setTarget(command_.roll_deg); + s_pitch_.setTarget(command_.pitch_deg); + s_yaw_.setTarget(command_.yaw_deg); + s_x_.setTarget(command_.x_mm); + s_y_.setTarget(command_.y_mm); + s_z_.setTarget(command_.z_mm); + + s_roll_.update(dt); s_pitch_.update(dt); s_yaw_.update(dt); + s_x_.update(dt); s_y_.update(dt); s_z_.update(dt); +} + +// Combines the commanded (smoothed) tilt with the robot's own stabilization +// correction, then applies both to a body-frame foot target so the feet stay +// planted while the body tilts/shifts on top of them. +// +// Sign convention here (whether positive roll tips the body left or right, +// whether offsetting +X moves the feet the way you expect) depends on your +// leg numbering and mount angles - bench-check it the same way as the IK +// elbow direction, and flip signs if it goes the wrong way. +FootTarget BodyPose::apply(const FootTarget& in) const { + float roll = radians(s_roll_.current + correction_roll_); + float pitch = radians(s_pitch_.current + correction_pitch_); + float yaw = radians(s_yaw_.current); + + float x = in.x, y = in.y, z = in.z; + + // yaw (about Z) + float x1 = x * cos(yaw) - y * sin(yaw); + float y1 = x * sin(yaw) + y * cos(yaw); + // pitch (about Y) + float x2 = x1 * cos(pitch) + z * sin(pitch); + float z2 = -x1 * sin(pitch) + z * cos(pitch); + // roll (about X) + float y3 = y1 * cos(roll) - z2 * sin(roll); + float z3 = y1 * sin(roll) + z2 * cos(roll); + + FootTarget out; + out.x = x2 - s_x_.current; + out.y = y3 - s_y_.current; + out.z = z3 - s_z_.current; + return out; +} diff --git a/2026-current/robot/BodyPose.h b/2026-current/robot/BodyPose.h new file mode 100644 index 0000000..f06ace9 --- /dev/null +++ b/2026-current/robot/BodyPose.h @@ -0,0 +1,36 @@ +#pragma once +#include "SmoothedValue.h" +#include "Kinematics.h" +#include "Config.h" + +struct PoseCommand { + float roll_deg = 0, pitch_deg = 0, yaw_deg = 0; + float x_mm = 0, y_mm = 0, z_mm = 0; +}; + +class BodyPose { +public: + void begin(); + void setCommand(const PoseCommand& c) { command_ = c; } + // From RobotIMU stabilization; pass 0,0 when the robot-gyro toggle is off. + void setCorrection(float roll_deg, float pitch_deg) { + correction_roll_ = roll_deg; + correction_pitch_ = pitch_deg; + } + + void update(float dt); + + // Applies the current (smoothed) pose transform to a body-frame foot target. + FootTarget apply(const FootTarget& in) const; + +private: + PoseCommand command_; + float correction_roll_ = 0, correction_pitch_ = 0; + + SmoothedValue s_roll_{POSE_TILT_MAX_RATE_DEG}; + SmoothedValue s_pitch_{POSE_TILT_MAX_RATE_DEG}; + SmoothedValue s_yaw_{POSE_TILT_MAX_RATE_DEG}; + SmoothedValue s_x_{POSE_OFFSET_MAX_RATE_MM}; + SmoothedValue s_y_{POSE_OFFSET_MAX_RATE_MM}; + SmoothedValue s_z_{POSE_OFFSET_MAX_RATE_MM}; +}; diff --git a/2026-current/robot/Comms.cpp b/2026-current/robot/Comms.cpp new file mode 100644 index 0000000..21089f7 --- /dev/null +++ b/2026-current/robot/Comms.cpp @@ -0,0 +1,28 @@ +#include "Comms.h" +#include "Config.h" +#include +#include + +static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN); + +bool Comms::begin() { + radio_present_ = radio.begin(); + if (radio_present_) { + radio.openReadingPipe(1, RADIO_ADDRESS); + radio.setPALevel(RF24_PA_LOW); + radio.setDataRate(RF24_250KBPS); + radio.setChannel(96); // pick one clear of your local Wi-Fi + radio.startListening(); + } + return radio_present_; +} + +void Comms::update() { + if (radio_present_) { + while (radio.available()) { + radio.read(&packet_, sizeof(packet_)); + last_rx_ms_ = millis(); + } + } + link_ok_ = (millis() - last_rx_ms_) < (uint32_t)(LINK_TIMEOUT_SEC * 1000); +} diff --git a/2026-current/robot/Comms.h b/2026-current/robot/Comms.h new file mode 100644 index 0000000..b29f220 --- /dev/null +++ b/2026-current/robot/Comms.h @@ -0,0 +1,40 @@ +#pragma once +#include + +// Remote -> robot, sent at a fixed rate. Kept as the single source of truth +// here so a future remote sketch just has to fill this same struct and +// radio.write() it - nothing on the robot side needs to change. +struct CmdPacket { + int16_t vx = 0, vy = 0; // left stick: strafe / forward-back, -1000..1000 + int16_t yaw_rate = 0; // left stick twist, -1000..1000 + int16_t rx = 0, ry = 0, rz = 0; // right stick: roll/pitch/yaw OR X/Y/height, per flags + uint8_t speed_scale = 128; // 0-255 + uint8_t flags = 0; // bit-packed, see FLAG_* below + uint8_t seq = 0; +}; + +constexpr uint8_t FLAG_ARMED = 1 << 0; +constexpr uint8_t FLAG_STAND = 1 << 1; // 0 = sit requested, 1 = stand requested +constexpr uint8_t FLAG_WALK = 1 << 2; // stick past deadzone -> walking +constexpr uint8_t FLAG_MODE_B = 1 << 3; // 0 = mode A direct, 1 = mode B heading-hold +constexpr uint8_t FLAG_RSTICK_XLATE = 1 << 4; // 0 = right stick = rotation, 1 = translation +constexpr uint8_t FLAG_GAIT_WAVE = 1 << 5; // 0 = tripod requested, 1 = wave requested +constexpr uint8_t FLAG_REMOTE_GYRO = 1 << 6; // remote tilt feedforward enabled +constexpr uint8_t FLAG_ROBOT_GYRO = 1 << 7; // robot stabilization feedback enabled + +class Comms { +public: + bool begin(); + void update(); // call every loop; polls the radio if present + bool linkOk() const { return link_ok_; } + const CmdPacket& latest() const { return packet_; } + + // Lets the serial test harness (or, later, real radio) inject a packet directly. + void injectPacket(const CmdPacket& p) { packet_ = p; last_rx_ms_ = millis(); } + +private: + CmdPacket packet_; + uint32_t last_rx_ms_ = 0; + bool link_ok_ = false; + bool radio_present_ = false; +}; diff --git a/2026-current/robot/Config.h b/2026-current/robot/Config.h new file mode 100644 index 0000000..2d67109 --- /dev/null +++ b/2026-current/robot/Config.h @@ -0,0 +1,59 @@ +#pragma once +#include + +// ===================================================================== +// Everything in this file is a placeholder for a typical small hexapod. +// Measure your own robot and edit these before trusting it on the floor. +// ===================================================================== + +// ---- Leg geometry ---- +constexpr int NUM_LEGS = 6; +// Leg order: 0=front-right, 1=mid-right, 2=rear-right, 3=rear-left, 4=mid-left, 5=front-left +// Angles measured counterclockwise from the body's +X (forward) axis, viewed from above. +constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -45, -90, -135, 135, 90, 45 }; +constexpr float LEG_MOUNT_X_MM[NUM_LEGS] = { 80, 0, -80, -80, 0, 80 }; +constexpr float LEG_MOUNT_Y_MM[NUM_LEGS] = { -60, -90, -60, 60, 90, 60 }; + +constexpr float COXA_LEN_MM = 30.0f; +constexpr float FEMUR_LEN_MM = 60.0f; +constexpr float TIBIA_LEN_MM = 100.0f; + +// ---- Joint limits, degrees ---- +constexpr float COXA_MIN_DEG = -60, COXA_MAX_DEG = 60; +constexpr float FEMUR_MIN_DEG = -80, FEMUR_MAX_DEG = 80; +constexpr float TIBIA_MIN_DEG = -150, TIBIA_MAX_DEG = -20; + +// ---- Neutral stance, relative to each leg's own coxa pivot ---- +constexpr float STANCE_RADIUS_MM = 140.0f; // how far out the foot rests +constexpr float STANCE_HEIGHT_MM = 90.0f; // how far below the coxa pivot the foot rests + +// ---- Servo pins (Teensy 4.0). Verify these are PWM-capable on your board. ---- +// Order per leg: coxa, femur, tibia. +constexpr uint8_t SERVO_PINS[NUM_LEGS][3] = { + { 2, 3, 4 }, // leg 0 + { 5, 6, 7 }, // leg 1 + { 8, 9, 10 }, // leg 2 + { 11, 12, 24 }, // leg 3 + { 25, 26, 27 }, // leg 4 + { 28, 29, 30 }, // leg 5 +}; +// Microseconds at 0deg and per additional degree. Calibrate per servo - they vary. +constexpr int SERVO_US_AT_0DEG = 1500; +constexpr int SERVO_US_PER_DEG = 10; + +// ---- Gait timing ---- +constexpr float DEFAULT_CYCLE_SEC = 0.8f; // one full gait cycle at neutral speed +constexpr float SWING_LIFT_MM = 35.0f; + +// ---- Control loop ---- +constexpr float CONTROL_HZ = 60.0f; + +// ---- Radio (nRF24L01) ---- +constexpr uint8_t RADIO_CE_PIN = 33; +constexpr uint8_t RADIO_CSN_PIN = 34; +constexpr uint8_t RADIO_ADDRESS[6] = "HXPD1"; // must match the remote once it exists +constexpr float LINK_TIMEOUT_SEC = 0.3f; + +// ---- Setpoint smoothing rates ---- +constexpr float POSE_TILT_MAX_RATE_DEG = 120.0f; // roll/pitch/yaw offset, deg/s +constexpr float POSE_OFFSET_MAX_RATE_MM = 150.0f; // X/Y/Z offset, mm/s diff --git a/2026-current/robot/GaitEngine.cpp b/2026-current/robot/GaitEngine.cpp new file mode 100644 index 0000000..9e2c6d9 --- /dev/null +++ b/2026-current/robot/GaitEngine.cpp @@ -0,0 +1,65 @@ +#include "GaitEngine.h" + +const GaitPattern GAIT_TRIPOD = { + 0.5f, + { 0.5f, 0.0f, 0.5f, 0.0f, 0.5f, 0.0f } +}; + +const GaitPattern GAIT_WAVE = { + 5.0f / 6.0f, + { 0.0f, 1.0f / 6, 2.0f / 6, 3.0f / 6, 4.0f / 6, 5.0f / 6 } +}; + +void GaitEngine::begin() { + for (int i = 0; i < 6; i++) { + legs_pattern_[i] = requested_pattern_; + leg_phase_[i] = legs_pattern_[i]->leg_phase_offset[i]; + } +} + +static float smoothstep(float t) { return t * t * (3.0f - 2.0f * t); } + +void GaitEngine::update(float dt, const Twist& twist, FootTarget out[6]) { + for (int i = 0; i < 6; i++) { + leg_phase_[i] += dt / cycle_sec_; + + // Touchdown: this leg's cycle wraps back to 0 - the one moment it's safe + // to hand it a newly requested pattern, since it's grounded with zero + // relative velocity right here regardless of which two patterns are involved. + if (leg_phase_[i] >= 1.0f) { + leg_phase_[i] -= 1.0f; + legs_pattern_[i] = requested_pattern_; + } + + const GaitPattern* p = legs_pattern_[i]; + bool in_stance = leg_phase_[i] < p->duty_factor; + + FootTarget neutral = Kinematics::neutralStance(i); + float stride_time = cycle_sec_ * p->duty_factor; + + // Half-stride displacement from the commanded twist. + float half_len_x = twist.vx * stride_time * 0.5f; + float half_len_y = twist.vy * stride_time * 0.5f; + // Rough yaw contribution using this leg's mount position as the moment arm. + float yaw_dx = -twist.omega * LEG_MOUNT_Y_MM[i] * stride_time * 0.5f; + float yaw_dy = twist.omega * LEG_MOUNT_X_MM[i] * stride_time * 0.5f; + + FootTarget start = { neutral.x - half_len_x - yaw_dx, neutral.y - half_len_y - yaw_dy, neutral.z }; + FootTarget end = { neutral.x + half_len_x + yaw_dx, neutral.y + half_len_y + yaw_dy, neutral.z }; + + FootTarget foot; + if (in_stance) { + float t = leg_phase_[i] / p->duty_factor; + foot.x = start.x + (end.x - start.x) * t; + foot.y = start.y + (end.y - start.y) * t; + foot.z = neutral.z; + } else { + float t = (leg_phase_[i] - p->duty_factor) / (1.0f - p->duty_factor); + float st = smoothstep(t); + foot.x = end.x + (start.x - end.x) * st; + foot.y = end.y + (start.y - end.y) * st; + foot.z = neutral.z + SWING_LIFT_MM * sin(PI * t); + } + out[i] = foot; + } +} diff --git a/2026-current/robot/GaitEngine.h b/2026-current/robot/GaitEngine.h new file mode 100644 index 0000000..94649cf --- /dev/null +++ b/2026-current/robot/GaitEngine.h @@ -0,0 +1,35 @@ +#pragma once +#include "Kinematics.h" +#include "Config.h" + +struct GaitPattern { + float duty_factor; // fraction of the cycle each leg spends in stance + float leg_phase_offset[6]; // each leg's phase offset within the cycle, 0..1 +}; + +extern const GaitPattern GAIT_TRIPOD; +extern const GaitPattern GAIT_WAVE; + +struct Twist { + float vx = 0, vy = 0; // mm/s, body frame + float omega = 0; // rad/s, body frame +}; + +class GaitEngine { +public: + void begin(); + void setRequestedPattern(const GaitPattern* p) { requested_pattern_ = p; } + void setCycleDuration(float sec) { cycle_sec_ = sec; } + + // Advances every leg by dt under the current twist and pattern, writing each + // leg's target foot position (body frame) into out[6]. + void update(float dt, const Twist& twist, FootTarget out[6]); + + bool legInStance(int i) const { return leg_phase_[i] < legs_pattern_[i]->duty_factor; } + +private: + float leg_phase_[6]; + const GaitPattern* legs_pattern_[6]; + const GaitPattern* requested_pattern_ = &GAIT_TRIPOD; + float cycle_sec_ = DEFAULT_CYCLE_SEC; +}; diff --git a/2026-current/robot/InputMapper.cpp b/2026-current/robot/InputMapper.cpp new file mode 100644 index 0000000..8252a04 --- /dev/null +++ b/2026-current/robot/InputMapper.cpp @@ -0,0 +1,60 @@ +#include "InputMapper.h" +#include + +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; +} diff --git a/2026-current/robot/InputMapper.h b/2026-current/robot/InputMapper.h new file mode 100644 index 0000000..e19ef4d --- /dev/null +++ b/2026-current/robot/InputMapper.h @@ -0,0 +1,30 @@ +#pragma once +#include "Comms.h" +#include "GaitEngine.h" +#include "BodyPose.h" + +struct MapperOutput { + Twist twist; + PoseCommand pose; + bool armed = false; + bool stand_request = false; + bool walk_active = false; + bool mode_b = false; + bool gait_wave = false; + bool remote_gyro_enabled = false; + bool robot_gyro_enabled = false; +}; + +class InputMapper { +public: + // heading_deg is the robot's current tracked heading estimate; only mode B uses it. + MapperOutput map(const CmdPacket& p, float heading_deg, float dt); + +private: + float heading_target_deg_ = 0; + static constexpr float MAX_TURN_RATE_DEG_S = 90.0f; + static constexpr float HEADING_KP = 2.0f; + static constexpr float MAX_LINEAR_MM_S = 150.0f; // tune to your leg workspace / servo speed + static constexpr float MAX_POSE_OFFSET_MM = 40.0f; + static constexpr float MAX_TILT_DEG = 20.0f; +}; diff --git a/2026-current/robot/Kinematics.cpp b/2026-current/robot/Kinematics.cpp new file mode 100644 index 0000000..c5072c7 --- /dev/null +++ b/2026-current/robot/Kinematics.cpp @@ -0,0 +1,67 @@ +#include "Kinematics.h" +#include "Config.h" + +// Standard analytical 3-DOF leg IK: coxa handles yaw in the horizontal plane, +// femur/tibia form a 2-link planar problem solved with the law of cosines. +// +// The elbow direction (which way tibia_rad's sign comes out) and the overall +// roll/pitch sign convention both depend on which way your leg is physically +// mounted - bench-check this on one leg with known foot positions before +// trusting all six. +JointAngles Kinematics::solve(int i, const FootTarget& target) { + JointAngles out{}; + + // Body-frame target -> leg-local frame: translate to the coxa pivot, then + // rotate by -mount angle so the leg's local +X points along its resting + // outward direction. + float dx = target.x - LEG_MOUNT_X_MM[i]; + float dy = target.y - LEG_MOUNT_Y_MM[i]; + float a = radians(-LEG_MOUNT_ANGLE_DEG[i]); + float local_x = dx * cos(a) - dy * sin(a); + float local_y = dx * sin(a) + dy * cos(a); + float local_z = target.z; + + float coxa_rad = atan2(local_y, local_x); + + // Horizontal distance from the femur pivot (after the coxa segment) to the foot. + float horiz = sqrt(local_x * local_x + local_y * local_y) - COXA_LEN_MM; + float dist = sqrt(horiz * horiz + local_z * local_z); + + out.valid = true; + float max_reach = FEMUR_LEN_MM + TIBIA_LEN_MM; + float min_reach = fabs(FEMUR_LEN_MM - TIBIA_LEN_MM); + if (dist > max_reach) { dist = max_reach * 0.999f; out.valid = false; } + if (dist < min_reach) { dist = min_reach * 1.001f; out.valid = false; } + + // Law of cosines: knee (tibia) angle from the femur/tibia/foot-distance triangle. + float cos_tibia = (FEMUR_LEN_MM * FEMUR_LEN_MM + TIBIA_LEN_MM * TIBIA_LEN_MM - dist * dist) + / (2.0f * FEMUR_LEN_MM * TIBIA_LEN_MM); + cos_tibia = constrain(cos_tibia, -1.0f, 1.0f); + float tibia_rad = -(PI - acos(cos_tibia)); // knee-down convention; flip sign if yours mounts the other way + + // Femur angle: elevation to the foot, plus the triangle's femur-side angle. + float elevation = atan2(-local_z, horiz); + float cos_femur_tri = (FEMUR_LEN_MM * FEMUR_LEN_MM + dist * dist - TIBIA_LEN_MM * TIBIA_LEN_MM) + / (2.0f * FEMUR_LEN_MM * dist); + cos_femur_tri = constrain(cos_femur_tri, -1.0f, 1.0f); + float femur_rad = elevation + acos(cos_femur_tri); + + out.coxa_deg = degrees(coxa_rad); + out.femur_deg = degrees(femur_rad); + out.tibia_deg = degrees(tibia_rad); + + if (out.coxa_deg < COXA_MIN_DEG || out.coxa_deg > COXA_MAX_DEG) out.valid = false; + if (out.femur_deg < FEMUR_MIN_DEG || out.femur_deg > FEMUR_MAX_DEG) out.valid = false; + if (out.tibia_deg < TIBIA_MIN_DEG || out.tibia_deg > TIBIA_MAX_DEG) out.valid = false; + + return out; +} + +FootTarget Kinematics::neutralStance(int i) { + float a = radians(LEG_MOUNT_ANGLE_DEG[i]); + FootTarget t; + t.x = LEG_MOUNT_X_MM[i] + cos(a) * (COXA_LEN_MM + STANCE_RADIUS_MM); + t.y = LEG_MOUNT_Y_MM[i] + sin(a) * (COXA_LEN_MM + STANCE_RADIUS_MM); + t.z = -STANCE_HEIGHT_MM; + return t; +} diff --git a/2026-current/robot/Kinematics.h b/2026-current/robot/Kinematics.h new file mode 100644 index 0000000..126ebe1 --- /dev/null +++ b/2026-current/robot/Kinematics.h @@ -0,0 +1,22 @@ +#pragma once +#include + +struct JointAngles { + float coxa_deg; + float femur_deg; + float tibia_deg; + bool valid; // false if the target was unreachable or outside joint limits +}; + +struct FootTarget { + float x, y, z; // body-frame mm, relative to body center +}; + +class Kinematics { +public: + // Solves leg i's joint angles for a body-frame foot target. + static JointAngles solve(int leg_index, const FootTarget& target); + + // The leg's resting/neutral foot target, in body frame. + static FootTarget neutralStance(int leg_index); +}; diff --git a/2026-current/robot/Limits.cpp b/2026-current/robot/Limits.cpp new file mode 100644 index 0000000..0242f1a --- /dev/null +++ b/2026-current/robot/Limits.cpp @@ -0,0 +1,35 @@ +#include "Limits.h" +#include +#include + +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; +} diff --git a/2026-current/robot/Limits.h b/2026-current/robot/Limits.h new file mode 100644 index 0000000..894e30f --- /dev/null +++ b/2026-current/robot/Limits.h @@ -0,0 +1,15 @@ +#pragma once +#include "Kinematics.h" + +class Limits { +public: + // Clamps a foot target into a conservative safe envelope around the leg's neutral stance. + static FootTarget clampWorkspace(int leg_index, const FootTarget& target); + + // Rate-limits a joint angle change to the servo's real max speed. + static float clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt); + + // Coarse safety check: are enough legs grounded right now to be stable. + // A real support-polygon/center-of-mass check is a good later upgrade. + static bool stancePolygonOk(const bool in_stance[6]); +}; diff --git a/2026-current/robot/README.md b/2026-current/robot/README.md new file mode 100644 index 0000000..41b7cdf --- /dev/null +++ b/2026-current/robot/README.md @@ -0,0 +1,113 @@ +# Hexapod robot firmware (Teensy 4.0) + +This is the **robot-side** firmware only. It is fully functional on its own +(driven by the serial test harness below) and is already wired to receive +commands from a future remote over nRF24L01 — the packet format in +`Comms.h` is the contract the remote sketch must match. + +## Required libraries + +Install via Arduino Library Manager (or Teensyduino's manager): + +- **RF24** by TMRh20 — nRF24L01 radio driver +- **Servo** — bundled with Teensyduino, nothing to install + +## Before you trust this on the floor + +Everything in `Config.h` is a placeholder for a generic small hexapod. Measure +your own robot and edit: + +- `LEG_MOUNT_ANGLE_DEG` / `LEG_MOUNT_X_MM` / `LEG_MOUNT_Y_MM` — where each + coxa pivot actually sits on your chassis, and which way it points +- `COXA_LEN_MM` / `FEMUR_LEN_MM` / `TIBIA_LEN_MM` — your link lengths +- `COXA_MIN_DEG..TIBIA_MAX_DEG` — your servos' real safe range of motion +- `STANCE_RADIUS_MM` / `STANCE_HEIGHT_MM` — how far out/down you want the + resting foot position +- `SERVO_PINS` — must be PWM-capable pins on your Teensy 4.0 wiring +- `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` — calibrate per servo, they vary + batch to batch +- `RADIO_ADDRESS` / channel (set in `Comms.cpp`) — must match the remote + once it exists + +## Bench-check checklist (do this before the first stand-up) + +These are sign conventions that depend on which way your hardware is +physically mounted, and the code has a best-guess default that may be wrong +for your build: + +1. **IK elbow/tibia direction** (`Kinematics.cpp`) — command one leg to a + few known foot positions by hand (e.g. via the serial harness) and watch + whether the knee bends the way you expect. Flip the sign noted in the + comment on `tibia_rad` if it bends backwards. +2. **BodyPose roll/pitch sign** (`BodyPose.cpp`) — command a small roll/pitch + offset and verify the body leans the direction you asked for, not the + opposite. +3. **RobotIMU register map** (`RobotIMU.cpp`) — written for an MPU6050-style + chip (I2C addr `0x68`). If your gyro is a different part, its register + addresses and scale factors will differ; swap them in. +4. **Sit-pose assumption** — the built-in sit pose in `robot.ino` assumes all + six feet stay grounded and simply lowers the body (a 0.4x height blend). + If your chassis is meant to rest its belly on the ground instead, this + needs a different, sequenced approach (fold legs up rather than just + lowering). + +## Testing without a remote yet + +`robot.ino` has `#define TEST_SERIAL_INPUT` enabled by default. With that on, +open the Arduino Serial Monitor (115200 baud, no line ending needed — single +keystrokes) and use: + +| Key | Action | +|-----|--------| +| `a` | toggle armed | +| `s` | toggle sit / stand | +| `w` / `x` | walk forward / backward | +| `q` / `e` | strafe left / right | +| `z` / `c` | yaw left / right | +| `g` | toggle gait: tripod ↔ wave | +| `m` | toggle walk mode: direct (A) ↔ heading-hold (B) | +| space | zero all velocity (stop walking, stay in current state) | + +Each keystroke injects a `CmdPacket` through `Comms::injectPacket()` — the +exact same path a real remote's received radio packets go through, so the +rest of the pipeline (mapper → state machine → gait → pose → IK → servos) +runs identically to how it will with real hardware. Once you have a real +remote transmitting, just remove the `#define TEST_SERIAL_INPUT` line (or +leave it — injected packets simply won't happen if you don't type). + +Arm (`a`), then stand (`s`), before trying to walk — the state machine +enforces `DISARMED → SIT → STAND → WALKING` in order and will otherwise +ignore walk commands. + +## What's ready for the remote + +- `Comms.h`'s `CmdPacket` struct and `FLAG_*` bit constants are the exact + wire format the remote must send (as an RF24 payload to `RADIO_ADDRESS` + on the same channel/data-rate/PA-level set in `Comms::begin()`). +- Failsafe: if no packet arrives for `LINK_TIMEOUT_SEC`, `Comms::linkOk()` + goes false and the main loop falls back to a zeroed `CmdPacket`, which + disarms the robot. +- `FLAG_REMOTE_GYRO` / `FLAG_ROBOT_GYRO` independently gate whether the + remote's tilt feeds into body pose and whether the robot's own IMU + stabilizes it — matching the touchscreen toggles you described. +- `FLAG_RSTICK_XLATE` switches the right stick between rotation-axes and + translation-axes interpretation, and `FLAG_MODE_B` switches between the + two walk modes, all inside the existing `InputMapper`. + +## File layout + +``` +robot/ +├── robot.ino - main sketch: setup/loop, state wiring, serial test harness +├── Config.h - all placeholder constants (edit this first) +├── SmoothedValue.h - generic rate-limited setpoint smoother +├── Kinematics.h/.cpp - per-leg 3-DOF analytical IK +├── GaitEngine.h/.cpp - tripod/wave gait, per-leg phase, dynamic gait swap +├── BodyPose.h/.cpp - 6-axis body pose composition (feedforward + IMU correction) +├── RobotIMU.h/.cpp - gyro/accel read + complementary filter +├── StateMachine.h/.cpp - DISARMED/SIT/STAND/WALKING with smoothed sit-stand blend +├── InputMapper.h/.cpp - CmdPacket -> twist/pose/mode decoding +├── Limits.h/.cpp - workspace clamp, joint rate limit, stance polygon check +├── Comms.h/.cpp - nRF24L01 receive + packet/flags definition +└── ServoDriver.h/.cpp - servo attach/relax/write +``` diff --git a/2026-current/robot/RobotIMU.cpp b/2026-current/robot/RobotIMU.cpp new file mode 100644 index 0000000..13eff8b --- /dev/null +++ b/2026-current/robot/RobotIMU.cpp @@ -0,0 +1,51 @@ +#include "RobotIMU.h" +#include + +// MPU6050-style register map, default +/-2g / +/-250deg/s ranges. +// Swap readRaw() if you're using a different IMU part. +static constexpr uint8_t IMU_ADDR = 0x68; +static constexpr uint8_t REG_PWR_MGMT_1 = 0x6B; +static constexpr uint8_t REG_ACCEL_XOUT_H = 0x3B; + +bool RobotIMU::begin() { + Wire.begin(); + Wire.beginTransmission(IMU_ADDR); + Wire.write(REG_PWR_MGMT_1); + Wire.write(0x00); // wake the IMU out of sleep + ok_ = (Wire.endTransmission() == 0); + return ok_; +} + +bool RobotIMU::readRaw(float& ax, float& ay, float& az, float& gx, float& gy, float& gz) { + Wire.beginTransmission(IMU_ADDR); + Wire.write(REG_ACCEL_XOUT_H); + if (Wire.endTransmission(false) != 0) return false; + Wire.requestFrom((int)IMU_ADDR, 14); + if (Wire.available() < 14) return false; + + int16_t raw_ax = (Wire.read() << 8) | Wire.read(); + int16_t raw_ay = (Wire.read() << 8) | Wire.read(); + int16_t raw_az = (Wire.read() << 8) | Wire.read(); + Wire.read(); Wire.read(); // skip temperature + int16_t raw_gx = (Wire.read() << 8) | Wire.read(); + int16_t raw_gy = (Wire.read() << 8) | Wire.read(); + int16_t raw_gz = (Wire.read() << 8) | Wire.read(); + + ax = raw_ax / 16384.0f; ay = raw_ay / 16384.0f; az = raw_az / 16384.0f; + gx = raw_gx / 131.0f; gy = raw_gy / 131.0f; gz = raw_gz / 131.0f; // deg/s + (void)gz; + return true; +} + +void RobotIMU::update(float dt) { + if (!ok_ || !enabled) return; + float ax, ay, az, gx, gy, gz; + if (!readRaw(ax, ay, az, gx, gy, gz)) return; + + float accel_roll = degrees(atan2(ay, az)); + float accel_pitch = degrees(atan2(-ax, sqrt(ay * ay + az * az))); + + constexpr float ALPHA = 0.98f; // trust the gyro short-term, the accelerometer long-term + roll_deg_ = ALPHA * (roll_deg_ + gx * dt) + (1 - ALPHA) * accel_roll; + pitch_deg_ = ALPHA * (pitch_deg_ + gy * dt) + (1 - ALPHA) * accel_pitch; +} diff --git a/2026-current/robot/RobotIMU.h b/2026-current/robot/RobotIMU.h new file mode 100644 index 0000000..a9c9920 --- /dev/null +++ b/2026-current/robot/RobotIMU.h @@ -0,0 +1,18 @@ +#pragma once +#include + +class RobotIMU { +public: + bool begin(); // returns false if the IMU didn't respond + void update(float dt); // no-op if disabled or the IMU never came up + float rollDeg() const { return roll_deg_; } + float pitchDeg() const { return pitch_deg_; } + + bool enabled = true; // robot-gyro toggle from CmdPacket sets this + +private: + float roll_deg_ = 0, pitch_deg_ = 0; + bool ok_ = false; + + bool readRaw(float& ax, float& ay, float& az, float& gx, float& gy, float& gz); +}; diff --git a/2026-current/robot/ServoDriver.cpp b/2026-current/robot/ServoDriver.cpp new file mode 100644 index 0000000..a6a1f1f --- /dev/null +++ b/2026-current/robot/ServoDriver.cpp @@ -0,0 +1,28 @@ +#include "ServoDriver.h" +#include "Config.h" +#include + +static Servo servos[NUM_LEGS][3]; + +void ServoDriver::begin() { + for (int i = 0; i < NUM_LEGS; i++) + for (int j = 0; j < 3; j++) + servos[i][j].attach(SERVO_PINS[i][j]); +} + +void ServoDriver::relaxAll() { + for (int i = 0; i < NUM_LEGS; i++) + for (int j = 0; j < 3; j++) + servos[i][j].detach(); +} + +static int degToMicros(float deg) { + return SERVO_US_AT_0DEG + (int)(deg * SERVO_US_PER_DEG); +} + +void ServoDriver::writeLeg(int i, const JointAngles& a) { + if (!a.valid) return; // never write an out-of-range or unreachable solution + servos[i][0].writeMicroseconds(degToMicros(a.coxa_deg)); + servos[i][1].writeMicroseconds(degToMicros(a.femur_deg)); + servos[i][2].writeMicroseconds(degToMicros(a.tibia_deg)); +} diff --git a/2026-current/robot/ServoDriver.h b/2026-current/robot/ServoDriver.h new file mode 100644 index 0000000..3589efc --- /dev/null +++ b/2026-current/robot/ServoDriver.h @@ -0,0 +1,9 @@ +#pragma once +#include "Kinematics.h" + +class ServoDriver { +public: + void begin(); // attach all servos + void relaxAll(); // detach all servos - call when disarming + void writeLeg(int leg_index, const JointAngles& angles); +}; diff --git a/2026-current/robot/SmoothedValue.h b/2026-current/robot/SmoothedValue.h new file mode 100644 index 0000000..c2931fc --- /dev/null +++ b/2026-current/robot/SmoothedValue.h @@ -0,0 +1,25 @@ +#pragma once + +// Rate-limited setpoint smoother. Every place a discrete command can jump +// (body pose axes, gait speed, the sit/stand blend) gets one of these instead +// of using the raw commanded value directly. +struct SmoothedValue { + float current = 0; + float target = 0; + float max_rate; // units per second + + explicit SmoothedValue(float rate = 100.0f) : max_rate(rate) {} + + void setTarget(float t) { target = t; } + + float update(float dt) { + float delta = target - current; + float max_step = max_rate * dt; + if (delta > max_step) delta = max_step; + else if (delta < -max_step) delta = -max_step; + current += delta; + return current; + } + + void reset(float v) { current = target = v; } +}; diff --git a/2026-current/robot/StateMachine.cpp b/2026-current/robot/StateMachine.cpp new file mode 100644 index 0000000..c565950 --- /dev/null +++ b/2026-current/robot/StateMachine.cpp @@ -0,0 +1,22 @@ +#include "StateMachine.h" +#include + +void StateMachine::update(bool armed, bool stand_request, bool walk_active, float dt) { + if (!armed) { + state_ = RobotState::DISARMED; + blend_ = 0; + return; + } + if (state_ == RobotState::DISARMED) { + state_ = RobotState::SIT; // arming always lands in sit first, never straight to stand + } + + float target_blend = stand_request ? 1.0f : 0.0f; + float step = BLEND_RATE * dt; + if (blend_ < target_blend) blend_ = min(blend_ + step, target_blend); + else if (blend_ > target_blend) blend_ = max(blend_ - step, target_blend); + + if (blend_ <= 0.001f) state_ = RobotState::SIT; + else if (blend_ >= 0.999f) state_ = walk_active ? RobotState::WALKING : RobotState::STAND; + else state_ = RobotState::STAND; // mid-transition +} diff --git a/2026-current/robot/StateMachine.h b/2026-current/robot/StateMachine.h new file mode 100644 index 0000000..38cd833 --- /dev/null +++ b/2026-current/robot/StateMachine.h @@ -0,0 +1,17 @@ +#pragma once + +enum class RobotState { DISARMED, SIT, STAND, WALKING }; + +class StateMachine { +public: + void begin() { state_ = RobotState::DISARMED; blend_ = 0; } + void update(bool armed_request, bool stand_request, bool walk_active_request, float dt); + + RobotState state() const { return state_; } + float sitStandBlend() const { return blend_; } // 0 = sit pose, 1 = stand pose + +private: + RobotState state_ = RobotState::DISARMED; + float blend_ = 0; + static constexpr float BLEND_RATE = 1.0f; // per second - full sit->stand in ~1s +}; diff --git a/2026-current/robot/robot.ino b/2026-current/robot/robot.ino new file mode 100644 index 0000000..01466b1 --- /dev/null +++ b/2026-current/robot/robot.ino @@ -0,0 +1,165 @@ +#include "Config.h" +#include "Kinematics.h" +#include "GaitEngine.h" +#include "BodyPose.h" +#include "RobotIMU.h" +#include "StateMachine.h" +#include "InputMapper.h" +#include "Limits.h" +#include "Comms.h" +#include "ServoDriver.h" + +// Uncomment to drive the robot from the Serial Monitor before a remote exists. +// See README.md for the key map. +#define TEST_SERIAL_INPUT + +GaitEngine gait; +BodyPose pose; +RobotIMU imu; +StateMachine state_machine; +InputMapper mapper; +Comms comms; +ServoDriver servo_driver; + +float current_heading_deg = 0; // open-loop: accumulates commanded omega. See note in loop(). +float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting +uint32_t last_loop_us = 0; +RobotState last_state = RobotState::DISARMED; + +#ifdef TEST_SERIAL_INPUT +CmdPacket test_packet; +void pollSerialTestInput() { + bool changed = false; + while (Serial.available()) { + char c = Serial.read(); + changed = true; + switch (c) { + case 'a': test_packet.flags ^= FLAG_ARMED; break; + case 's': test_packet.flags ^= FLAG_STAND; break; + case 'w': test_packet.vy = min(test_packet.vy + 200, 1000); test_packet.flags |= FLAG_WALK; break; + case 'x': test_packet.vy = max(test_packet.vy - 200, -1000); test_packet.flags |= FLAG_WALK; break; + case 'q': test_packet.vx = max(test_packet.vx - 200, -1000); test_packet.flags |= FLAG_WALK; break; + case 'e': test_packet.vx = min(test_packet.vx + 200, 1000); test_packet.flags |= FLAG_WALK; break; + case 'z': test_packet.yaw_rate = max(test_packet.yaw_rate - 200, -1000); test_packet.flags |= FLAG_WALK; break; + case 'c': test_packet.yaw_rate = min(test_packet.yaw_rate + 200, 1000); test_packet.flags |= FLAG_WALK; break; + case 'g': test_packet.flags ^= FLAG_GAIT_WAVE; break; + case 'm': test_packet.flags ^= FLAG_MODE_B; break; + case ' ': + test_packet.vx = 0; test_packet.vy = 0; test_packet.yaw_rate = 0; + test_packet.flags &= ~FLAG_WALK; + break; + default: changed = false; break; + } + } + if (changed) { + test_packet.speed_scale = 180; + comms.injectPacket(test_packet); + } +} +#endif + +void setup() { + Serial.begin(115200); + gait.begin(); + pose.begin(); + state_machine.begin(); + + if (!imu.begin()) Serial.println("robot IMU not responding - stabilization disabled"); + if (!comms.begin()) Serial.println("radio not responding - waiting for injected/test commands only"); + + servo_driver.begin(); + last_loop_us = micros(); +} + +void loop() { + uint32_t now_us = micros(); + float dt = (now_us - last_loop_us) / 1e6f; + if (dt < (1.0f / CONTROL_HZ)) return; // hold to the fixed control rate + last_loop_us = now_us; + +#ifdef TEST_SERIAL_INPUT + pollSerialTestInput(); +#endif + + comms.update(); + CmdPacket cmd = comms.linkOk() ? comms.latest() : CmdPacket(); // link lost -> all-neutral, unarmed + + MapperOutput mapped = mapper.map(cmd, current_heading_deg, dt); + + imu.enabled = mapped.robot_gyro_enabled; + imu.update(dt); + + // Simple open-loop heading estimate: accumulates the omega we've been + // commanding. Good enough for mode B in a normal session; fusing the + // robot IMU's yaw rate in as well is the more robust upgrade if legs + // ever slip on your surface. + current_heading_deg += degrees(mapped.twist.omega) * dt; + + state_machine.update(mapped.armed, mapped.stand_request, mapped.walk_active, dt); + RobotState state = state_machine.state(); + + // Relax servos on the way into DISARMED, reattach on the way out. + if (state == RobotState::DISARMED && last_state != RobotState::DISARMED) { + servo_driver.relaxAll(); + } else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) { + servo_driver.begin(); + } + last_state = state; + + gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD); + + // Remote-gyro toggle off -> drop commanded tilt but keep XYZ offset/height. + PoseCommand pose_cmd = mapped.pose; + if (!mapped.remote_gyro_enabled) { + pose_cmd.roll_deg = 0; + pose_cmd.pitch_deg = 0; + } + pose.setCommand(pose_cmd); + pose.setCorrection(mapped.robot_gyro_enabled ? -imu.rollDeg() : 0, + mapped.robot_gyro_enabled ? -imu.pitchDeg() : 0); + pose.update(dt); + + FootTarget gait_targets[NUM_LEGS]; + bool in_stance[NUM_LEGS]; + + if (state == RobotState::WALKING) { + gait.update(dt, mapped.twist, gait_targets); + for (int i = 0; i < NUM_LEGS; i++) in_stance[i] = gait.legInStance(i); + } else { + // Sit/stand: blend between a crouched and a neutral stance height, no + // stepping. This assumes "sit" keeps all six feet grounded - if your + // sit pose rests the chassis on the ground instead, replace this with + // a proper sequenced transition (feet positioned first, then lift). + float blend = state_machine.sitStandBlend(); + for (int i = 0; i < NUM_LEGS; i++) { + FootTarget neutral = Kinematics::neutralStance(i); + FootTarget sit_target = { neutral.x, neutral.y, neutral.z * 0.4f }; + gait_targets[i].x = sit_target.x + (neutral.x - sit_target.x) * blend; + gait_targets[i].y = sit_target.y + (neutral.y - sit_target.y) * blend; + gait_targets[i].z = sit_target.z + (neutral.z - sit_target.z) * blend; + in_stance[i] = true; + } + } + + bool stable = Limits::stancePolygonOk(in_stance); + (void)stable; // hook this up to your own response once you're ready - slow down, warn, etc. + + constexpr float MAX_JOINT_DEG_S = 300.0f; // measure your actual servo speed and set this for real + + for (int i = 0; i < NUM_LEGS; i++) { + FootTarget posed = pose.apply(gait_targets[i]); + FootTarget clamped = Limits::clampWorkspace(i, posed); + JointAngles angles = Kinematics::solve(i, clamped); + + angles.coxa_deg = Limits::clampJointRate(last_joint_deg[i][0], angles.coxa_deg, MAX_JOINT_DEG_S, dt); + angles.femur_deg = Limits::clampJointRate(last_joint_deg[i][1], angles.femur_deg, MAX_JOINT_DEG_S, dt); + angles.tibia_deg = Limits::clampJointRate(last_joint_deg[i][2], angles.tibia_deg, MAX_JOINT_DEG_S, dt); + last_joint_deg[i][0] = angles.coxa_deg; + last_joint_deg[i][1] = angles.femur_deg; + last_joint_deg[i][2] = angles.tibia_deg; + + if (state != RobotState::DISARMED) { + servo_driver.writeLeg(i, angles); + } + } +} diff --git a/2026-current/scripts/select_src_dir.py b/2026-current/scripts/select_src_dir.py new file mode 100644 index 0000000..0f52a6f --- /dev/null +++ b/2026-current/scripts/select_src_dir.py @@ -0,0 +1,16 @@ +# PlatformIO pre-build script: point each environment at its own sketch folder. +# +# PlatformIO only has one global `src_dir`, and it only converts .ino files in +# the root of that directory. Each env sets `custom_src_dir = robot` (or +# `remote`) and this script swaps PROJECT_SRC_DIR before the build starts, so +# robot/ and remote/ stay plain Arduino-style sketch folders (one .ino plus +# paired .h/.cpp files) that also open in the Arduino IDE. +import os + +Import("env") # noqa: F821 - injected by PlatformIO/SCons + +sub = env.GetProjectOption("custom_src_dir") # noqa: F821 +src = os.path.join(env.subst("$PROJECT_DIR"), sub) # noqa: F821 +if not os.path.isdir(src): + raise SystemExit("custom_src_dir '%s' does not exist" % src) +env.Replace(PROJECT_SRC_DIR=src) # noqa: F821