mangavalk
3d879ad803
Comments in Config.h and the README referred to LEGACY/, which is folded into the generation folders. Also 'from the repo root' -> 'from the 2026-current folder'. Comment and documentation changes only; approved by the owner.
131 lines
7.0 KiB
C++
131 lines
7.0 KiB
C++
#pragma once
|
|
#include <Arduino.h>
|
|
|
|
// =====================================================================
|
|
// 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 ----
|
|
// Taken from the Unity model (Hexapod_Simulation_Unity/Assets/Main.unity,
|
|
// 1 Unity unit = 10 mm): hip pivots at x = +-10 / 0, z = +-5 / +-6.5;
|
|
// coxa 5.5, femur 10, tibia 15. BENCH: confirm against the real chassis.
|
|
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.
|
|
// This is the direction the coxa points at servo center: straight sideways on this chassis.
|
|
constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -90, -90, -90, 90, 90, 90 };
|
|
constexpr float LEG_MOUNT_X_MM[NUM_LEGS] = { 100, 0, -100, -100, 0, 100 };
|
|
constexpr float LEG_MOUNT_Y_MM[NUM_LEGS] = { -50, -65, -50, 50, 65, 50 };
|
|
|
|
constexpr float COXA_LEN_MM = 55.0f;
|
|
constexpr float FEMUR_LEN_MM = 100.0f;
|
|
constexpr float TIBIA_LEN_MM = 150.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 ----
|
|
// From the Unity model's rest foot point: 130 mm out from the coxa pivot
|
|
// (= COXA_LEN_MM + 75), 150 mm below it.
|
|
constexpr float STANCE_RADIUS_MM = 75.0f; // how far out the foot rests, beyond the coxa
|
|
constexpr float STANCE_HEIGHT_MM = 150.0f; // how far below the coxa pivot the foot rests (default height)
|
|
// The remote's height slider (CmdPacket.body_height) moves the standing
|
|
// height by up to this much either way (100..200 mm here - both ends checked
|
|
// reachable in the simulation), at no more than STANCE_HEIGHT_RATE_MM_S.
|
|
constexpr float STANCE_HEIGHT_RANGE_MM = 50.0f;
|
|
constexpr float STANCE_HEIGHT_RATE_MM_S = 60.0f;
|
|
|
|
// ---- Leg collision ----
|
|
// Minimum distance between the center lines of neighbouring legs' segments
|
|
// (coxa, femur, tibia). Gait strides and body poses are scaled down to keep
|
|
// it. BENCH: set to roughly the width of a leg (servo body + bracket).
|
|
constexpr float LEG_CLEARANCE_MM = 30.0f;
|
|
|
|
// ---- Servos (Teensy 3.6, hardware PWM at SERVO_PWM_HZ) ----
|
|
// One entry per servo: pulse = us_at_0deg + us_per_deg * joint_deg, clamped
|
|
// to us_min..us_max. us_per_deg is signed - its sign is the servo's direction.
|
|
// Joint angles are this firmware's convention (see Kinematics.cpp): coxa
|
|
// + = CCW from the mount angle, femur + = up, tibia relative to femur, + = up.
|
|
struct ServoCal {
|
|
uint8_t pin;
|
|
float us_at_0deg;
|
|
float us_per_deg;
|
|
int16_t us_min, us_max;
|
|
};
|
|
|
|
// Built from the old robot sketch (2019-teensy-nrf24/Hexapod_Teensy/config.h), which
|
|
// drove each servo with PWMServo: a command of 0..180 maps linearly onto
|
|
// min..max microseconds (servoMin/servoMax + servoCallibration trim). The
|
|
// command for a joint angle is zero + dir * joint_deg + offset, where
|
|
// zero/dir come from the old Unity Leg.cs servo mapping and offset is the
|
|
// old servoOffset[]:
|
|
// coxa 90 - coxa (both sides)
|
|
// femur 90 + femur right, 90 - femur left
|
|
// tibia 0 - tibia right, 180 + tibia left (90 = knee bent 90 deg)
|
|
constexpr ServoCal oldServo(uint8_t pin, int min, int max, int trim, float zero, int dir, float offset) {
|
|
return { pin,
|
|
(min + trim) + (max - min) / 180.0f * (zero + offset),
|
|
dir * (max - min) / 180.0f,
|
|
(int16_t)(min + trim), (int16_t)(max + trim) };
|
|
}
|
|
|
|
// Old ranges: coxa and tibia 553..2425 us, femur 840..2050 us over the 0..180
|
|
// command. Comments give the old servo index (old Unity Leg.startServo).
|
|
// BENCH:
|
|
// - The femur's narrower range means ~6.7 us per command degree vs ~10.4
|
|
// for coxa/tibia. Check one femur moves the angle the sim shows.
|
|
// - The old code had +90 / -90 coxa offsets on rear-left / front-left,
|
|
// which put those two servos at the end of their travel at rest (command
|
|
// 180 / 0), so they could only swing one way (the simulation showed
|
|
// those legs clamping as soon as the robot turned or posed). Owner's
|
|
// decision: re-mount those two horns 90 deg so the leg points straight
|
|
// out at servo center, like the others - offsets are 0 here. Do that
|
|
// before powering the robot with this firmware.
|
|
// - Check every direction on the bench, one joint at a time.
|
|
constexpr ServoCal SERVOS[NUM_LEGS][3] = {
|
|
// leg 0 front-right (old 6-8)
|
|
{ oldServo(10, 553, 2425, 0, 90, -1, 0), oldServo( 9, 840, 2050, 80, 90, +1, 0), oldServo( 8, 553, 2425, -50, 0, -1, 0) },
|
|
// leg 1 mid-right (old 3-5)
|
|
{ oldServo( 7, 553, 2425, 0, 90, -1, 0), oldServo( 6, 840, 2050, 100, 90, +1, 0), oldServo( 5, 553, 2425, -30, 0, -1, 0) },
|
|
// leg 2 rear-right (old 0-2)
|
|
{ oldServo( 4, 553, 2425, 0, 90, -1, 0), oldServo( 3, 840, 2050, 100, 90, +1, 0), oldServo( 2, 553, 2425, 0, 0, -1, 0) },
|
|
// leg 3 rear-left (old 9-11). Coxa offset was +90 in the old code: horn to be re-mounted (see above).
|
|
{ oldServo(23, 553, 2425, -120, 90, -1, 0), oldServo(22, 840, 2050, 0, 90, -1, 0), oldServo(21, 553, 2425, 0, 180, +1, 0) },
|
|
// leg 4 mid-left (old 12-14)
|
|
{ oldServo(20, 553, 2425, -10, 90, -1, 0), oldServo(36, 840, 2050, 20, 90, -1, 0), oldServo(37, 553, 2425, 0, 180, +1, 0) },
|
|
// leg 5 front-left (old 15-17). Coxa offset was -90 in the old code: horn to be re-mounted (see above).
|
|
{ oldServo(17, 553, 2425, 0, 90, -1, 0), oldServo(16, 840, 2050, 20, 90, -1, 0), oldServo(38, 553, 2425, -80, 180, +1, 0) },
|
|
};
|
|
constexpr float SERVO_PWM_HZ = 50.0f;
|
|
|
|
// ---- 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) ----
|
|
// Address/channel/data rate/PA/payload size live in shared/HexapodProtocol
|
|
// so the robot and remote can't drift apart. Only this board's wiring is here.
|
|
// From the old robot sketch (2019-teensy-nrf24/Hexapod_Teensy/Nrf.h): SPI on MOSI 11 /
|
|
// MISO 12 with SCK moved to its alternate pin 14 (Teensy 3.x), so pin 13
|
|
// stays free.
|
|
constexpr uint8_t RADIO_CE_PIN = 39;
|
|
constexpr uint8_t RADIO_CSN_PIN = 15;
|
|
constexpr uint8_t RADIO_SCK_PIN = 14;
|
|
constexpr float LINK_TIMEOUT_SEC = 0.3f;
|
|
|
|
// ---- Robot-gyro self-leveling ----
|
|
// Integral gain (1/s): a leftover tilt of 1 deg changes the correction by
|
|
// this many deg/s. Higher levels faster but can wobble. BENCH: tune on the robot.
|
|
constexpr float ROBOT_GYRO_KI = 3.0f;
|
|
constexpr float ROBOT_GYRO_MAX_CORR_DEG = 15.0f; // steepest slope it will level out
|
|
|
|
// ---- 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
|