Robot: Teensy 3.6 with the old robot's servo wiring and calibration
- LEGACY/Hexapod_Teensy/: old robot sketch (reference only). - platformio.ini: robot env targets teensy36 (owner-confirmed board). - Config.h: per-servo SERVOS table (pin, pulse at 0 deg, signed us/deg, pulse limits) built from the old sketch's pins, ranges, trims and the old Unity servo directions. Rear-left/front-left coxa offsets set to 0: the owner re-horns those two servos (the old +-90 left them at the end of travel; the sim showed them clamping). - ServoDriver: 18 servos on hardware PWM (50 Hz, 16-bit); the Teensy 3.6 Servo library stops at 12. - Radio on CE 39 / CSN 15 with SPI SCK on pin 14. - sim: analogWrite/PWM shims, pulses decoded through SERVOS (API v6). - Docs: CLAUDE.md, READMEs. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
a961fa721b
commit
5ff85c4eb8
21 files changed
+795
-80
No files matched your search
@@ -9,6 +9,7 @@ static_assert(HXP_RADIO_DATA_RATE == RF24_250KBPS && HXP_RADIO_PA_LEVEL == RF24_
|
||||
static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);
|
||||
|
||||
bool Comms::begin() {
|
||||
SPI.setSCK(RADIO_SCK_PIN); // alternate SCK pin (see Config.h); before RF24 starts SPI
|
||||
radio_present_ = radio.begin();
|
||||
if (radio_present_) {
|
||||
radio.setPayloadSize(HXP_RADIO_PAYLOAD); // mismatched protocol versions won't be received at all
|
||||
|
||||
+61
-14
@@ -44,19 +44,62 @@ constexpr float STANCE_HEIGHT_RATE_MM_S = 60.0f;
|
||||
// it. BENCH: set to roughly the width of a leg (servo body + bracket).
|
||||
constexpr float LEG_CLEARANCE_MM = 30.0f;
|
||||
|
||||
// ---- 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
|
||||
// ---- 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;
|
||||
};
|
||||
// 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;
|
||||
|
||||
// Built from the old robot sketch (LEGACY/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
|
||||
@@ -68,8 +111,12 @@ 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.
|
||||
constexpr uint8_t RADIO_CE_PIN = 33;
|
||||
constexpr uint8_t RADIO_CSN_PIN = 34;
|
||||
// From the old robot sketch (LEGACY/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 ----
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
# Hexapod robot firmware (Teensy 4.0)
|
||||
# Hexapod robot firmware (Teensy 3.6)
|
||||
|
||||
This is the **robot-side** firmware. It is fully functional on its own
|
||||
(driven by the serial test harness below) and receives commands from the
|
||||
@@ -10,8 +10,8 @@ sketches compile against.
|
||||
|
||||
From the repo root (PlatformIO): `pio run -e robot`, flash with
|
||||
`pio run -e robot -t upload`, monitor with `pio device monitor -e robot`.
|
||||
RF24 is pulled in automatically via `lib_deps`; Servo ships with the Teensy
|
||||
framework.
|
||||
RF24 is pulled in automatically via `lib_deps`. Servos are driven by the
|
||||
Teensy's hardware PWM directly, so no servo library is needed.
|
||||
|
||||
Arduino IDE also works if you copy `shared/HexapodProtocol/` into your
|
||||
sketchbook `libraries/` folder and install **RF24** by TMRh20.
|
||||
@@ -31,15 +31,18 @@ still a placeholder. Check all of it against the real robot:
|
||||
- `LEG_CLEARANCE_MM`: how close neighbouring legs may get. Walking speed
|
||||
and body pose are scaled down to keep it, so set it to about one leg's
|
||||
width.
|
||||
- `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_CE_PIN` / `RADIO_CSN_PIN` — your nRF24 wiring. Address, channel,
|
||||
data rate, PA level and payload size are in `shared/HexapodProtocol` so
|
||||
they can't drift from the remote.
|
||||
- **Pin conflict:** `SERVO_PINS` leg 3 uses pins 11 and 12, which are the
|
||||
Teensy 4.0's SPI MOSI/MISO that the nRF24 needs. Move those two servos
|
||||
before wiring the radio.
|
||||
- `SERVOS`: one entry per servo with pin, pulse at 0°, signed µs per
|
||||
degree (sign = direction) and pulse limits. It's prefilled from the old
|
||||
robot sketch (`LEGACY/Hexapod_Teensy/config.h`): pins, ranges, trims, and
|
||||
directions from the old Unity servo mapping. Check every joint's
|
||||
direction on the bench.
|
||||
- **Before powering up:** re-mount the rear-left and front-left coxa servo
|
||||
horns 90° so those legs point straight out at servo center. The old code
|
||||
needed ±90° offsets there, which left the servos at the end of their
|
||||
travel.
|
||||
- `RADIO_CE_PIN` / `RADIO_CSN_PIN` / `RADIO_SCK_PIN`: 39 / 15 / 14, as in
|
||||
the old sketch. Address, channel, data rate, PA level and payload size
|
||||
are in `shared/HexapodProtocol` so they can't drift from the remote.
|
||||
|
||||
## Bench-check checklist (do this before the first stand-up)
|
||||
|
||||
@@ -69,12 +72,12 @@ for your build:
|
||||
each foot opposite to the commanded motion. The smoke test checks that
|
||||
`+vx` moves the body forward and `+yaw_rate` turns it left (CCW). On
|
||||
the bench, confirm `w` walks forward and `z` turns left.
|
||||
6. **Per-servo calibration**. `ServoDriver` uses one
|
||||
`SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` for all 18 servos, so servo
|
||||
center = joint angle 0 (tibia *straight*). On this build the knee servo
|
||||
center is the tibia at 90° to the femur, and the left side is mirrored
|
||||
(per the old Unity/servo code). That needs a per-joint offset and
|
||||
direction before the real robot will stand.
|
||||
6. **Per-servo calibration** (`Config.h` `SERVOS`). Arm the robot,
|
||||
stand it on a box, and move one joint at a time (or watch the sim next
|
||||
to it): each servo must turn the way the sim shows. To flip a direction,
|
||||
flip that entry's `dir`. Also check that a femur moves the commanded
|
||||
angle: its old range gives only ≈6.7 µs/° against ≈10.4 for coxa and
|
||||
tibia.
|
||||
|
||||
## Testing in the simulation
|
||||
|
||||
|
||||
@@ -1,28 +1,42 @@
|
||||
#include "ServoDriver.h"
|
||||
#include "Config.h"
|
||||
#include <Servo.h>
|
||||
|
||||
static Servo servos[NUM_LEGS][3];
|
||||
// 18 servos on hardware PWM, the way the old robot sketch drove them (the
|
||||
// Teensy 3.6 Servo library stops at 12). 16-bit duty at 50 Hz is ~0.3 us per
|
||||
// step. Every servo pin must be PWM-capable - on the Teensy 3.6 all of the
|
||||
// SERVOS[] pins are.
|
||||
static constexpr uint8_t PWM_BITS = 16;
|
||||
static constexpr float PERIOD_US = 1e6f / SERVO_PWM_HZ;
|
||||
|
||||
static void writePulse(uint8_t pin, float us) {
|
||||
analogWrite(pin, (int)(us / PERIOD_US * (1 << PWM_BITS) + 0.5f));
|
||||
}
|
||||
|
||||
void ServoDriver::begin() {
|
||||
analogWriteResolution(PWM_BITS);
|
||||
for (int i = 0; i < NUM_LEGS; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
servos[i][j].attach(SERVO_PINS[i][j]);
|
||||
for (int j = 0; j < 3; j++) {
|
||||
analogWriteFrequency(SERVOS[i][j].pin, SERVO_PWM_HZ);
|
||||
pinMode(SERVOS[i][j].pin, OUTPUT);
|
||||
analogWrite(SERVOS[i][j].pin, 0); // no pulses until the first writeLeg()
|
||||
}
|
||||
}
|
||||
|
||||
void ServoDriver::relaxAll() {
|
||||
for (int i = 0; i < NUM_LEGS; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
servos[i][j].detach();
|
||||
analogWrite(SERVOS[i][j].pin, 0); // no pulses: the servos go limp
|
||||
}
|
||||
|
||||
static int degToMicros(float deg) {
|
||||
return SERVO_US_AT_0DEG + (int)(deg * SERVO_US_PER_DEG);
|
||||
int ServoDriver::degToMicros(int leg, int joint, float deg) {
|
||||
const ServoCal& c = SERVOS[leg][joint];
|
||||
float us = c.us_at_0deg + c.us_per_deg * deg;
|
||||
return (int)constrain(us, (float)c.us_min, (float)c.us_max);
|
||||
}
|
||||
|
||||
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));
|
||||
writePulse(SERVOS[i][0].pin, degToMicros(i, 0, a.coxa_deg));
|
||||
writePulse(SERVOS[i][1].pin, degToMicros(i, 1, a.femur_deg));
|
||||
writePulse(SERVOS[i][2].pin, degToMicros(i, 2, a.tibia_deg));
|
||||
}
|
||||
@@ -3,7 +3,10 @@
|
||||
|
||||
class ServoDriver {
|
||||
public:
|
||||
void begin(); // attach all servos
|
||||
void relaxAll(); // detach all servos - call when disarming
|
||||
void begin(); // set up the PWM pins (no pulses yet)
|
||||
void relaxAll(); // stop all pulses - call when disarming
|
||||
void writeLeg(int leg_index, const JointAngles& angles);
|
||||
|
||||
// Joint angle -> pulse through that servo's calibration (Config.h SERVOS).
|
||||
static int degToMicros(int leg, int joint, float deg);
|
||||
};
|
||||
Reference in new issue
Block a user