mangavalkandClaude Opus 5.5
91b5b99fd5
hardware/openscad/ rebuilds the broken Fusion models as OpenSCAD: body (top/bottom plate, standoffs, battery bay, switch), legs (coxa bracket, femur link, tibia, horn adapters, TPU socks, servo cables), the whole robot, and the remote (shell, lid, Nextion 3.5", JH-D400X-R4 sticks, 18650 UPS board, IMU, Teensy 4.0, USB-C breakout). Every part was checked by slicing against the original STEP solids; the original's bugs are fixed and listed in PLANNING.md. The robot now uses a hexagon leg layout (corner legs 60 deg round from the mid legs; the corner coxa brackets need turning on their splines). Physical limits from a collision sweep of the model are in Config.h: per-leg coxa limits and LEG_CLEARANCE_MM 56 (a leg is 55.2 mm wide at the joints); the sim API exports them (version 7). sim_smoke passes. Unity loads the model (export_unity.py -> Resources/OpenScadModel, OpenScadModel.cs builds a new rig in place of the old scene's robot). The PCB model's mounting holes now use the exact EasyEDA grid. README shows renders of both. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
152 lines
8.5 KiB
C++
152 lines
8.5 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. Confirmed exactly against the real
|
|
// chassis's CAD (hardware/openscad, from the Fusion model): coxa axes at
|
|
// these mounts, coxa 55 / femur 100 / tibia 150 mm to the centre of the
|
|
// 10 mm foot ball. The femur and knee axes lie in the coxa pivot plane
|
|
// (Z = 0), 13.5 mm above the bottom plate's top face.
|
|
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. Hexagon layout (owner's
|
|
// request): mid legs straight sideways, front/rear legs 60 deg further round, so
|
|
// all six are 60 deg apart. BENCH: the corner coxa brackets sit 60 deg round on
|
|
// their servo splines (4 teeth of 24) compared with the old sideways layout.
|
|
constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -30, -90, -150, 150, 90, 30 };
|
|
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 ----
|
|
// Physical limits from the OpenSCAD model (hardware/openscad, collision
|
|
// sweep of the real parts, see PLANNING.md "Physical limits"), and servo
|
|
// travel (+-90 coxa/femur, -180..0 tibia):
|
|
// coxa mid legs -66.5 .. 66.5 coxa bracket shell hits the plate
|
|
// coxa 0 front-right, 3 rear-left -95.75 .. 66.75 (servo: -90)
|
|
// coxa 2 rear-right, 5 front-left -66.75 .. 95.75 (servo: 90)
|
|
// femur -101 .. 101 femur link hits the coxa bracket (servo: 90)
|
|
// tibia -177 .. 12.75 rod folds into the femur link / shell hits it (servo: 0)
|
|
// The limits below keep ~3 deg from those and the earlier software limits
|
|
// where those are tighter. Per leg, because the corner legs' ranges differ (in
|
|
// the old sideways layout they could only swing ~36 deg towards the mid leg).
|
|
constexpr float COXA_MIN_DEG[NUM_LEGS] = { -60, -60, -60, -60, -60, -60 };
|
|
constexpr float COXA_MAX_DEG[NUM_LEGS] = { 60, 60, 60, 60, 60, 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. From the OpenSCAD model: at the joints a leg reaches 33.7 mm to its
|
|
// horn side (rear) and 21.5 mm to its bearing side (front), so two
|
|
// neighbours touch at 55.2 mm between center lines.
|
|
constexpr float LEG_CLEARANCE_MM = 56.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
|