#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 ---- // 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