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
@@ -50,8 +50,6 @@ HXSIM_API void hxsim_get_geometry(HxGeometry* g) {
|
||||
g->femur_min_deg = FEMUR_MIN_DEG; g->femur_max_deg = FEMUR_MAX_DEG;
|
||||
g->tibia_min_deg = TIBIA_MIN_DEG; g->tibia_max_deg = TIBIA_MAX_DEG;
|
||||
g->control_hz = CONTROL_HZ;
|
||||
g->servo_us_at_0deg = SERVO_US_AT_0DEG;
|
||||
g->servo_us_per_deg = SERVO_US_PER_DEG;
|
||||
g->leg_clearance_mm = LEG_CLEARANCE_MM;
|
||||
g->stance_height_range_mm = STANCE_HEIGHT_RANGE_MM;
|
||||
}
|
||||
@@ -105,22 +103,24 @@ HXSIM_API int32_t hxsim_serial_output(char* out, int32_t cap) { return simhw::se
|
||||
HXSIM_API void hxsim_get_servos(int32_t* us, int32_t* attached, uint32_t* writes) {
|
||||
for (int i = 0; i < NUM_LEGS; i++)
|
||||
for (int j = 0; j < 3; j++) {
|
||||
const simhw::ServoPin& p = simhw::servoPin(SERVO_PINS[i][j]);
|
||||
const simhw::ServoPin& p = simhw::servoPin(SERVOS[i][j].pin);
|
||||
if (us) us[i * 3 + j] = p.us;
|
||||
if (attached) attached[i * 3 + j] = p.attached ? 1 : 0;
|
||||
if (writes) writes[i * 3 + j] = p.writes;
|
||||
}
|
||||
}
|
||||
|
||||
// Inverse of ServoDriver::degToMicros - what angle the firmware meant.
|
||||
static float pulseToDeg(int us) {
|
||||
return (us - SERVO_US_AT_0DEG) / (float)SERVO_US_PER_DEG;
|
||||
// Inverse of ServoDriver::degToMicros through the same per-servo table:
|
||||
// what angle the firmware meant by this pulse.
|
||||
static float pulseToDeg(int leg, int joint, int us) {
|
||||
const ServoCal& c = SERVOS[leg][joint];
|
||||
return (us - c.us_at_0deg) / c.us_per_deg;
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_get_joints_deg(float* deg) {
|
||||
for (int i = 0; i < NUM_LEGS; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
deg[i * 3 + j] = pulseToDeg(simhw::servoPin(SERVO_PINS[i][j]).us);
|
||||
deg[i * 3 + j] = pulseToDeg(i, j, simhw::servoPin(SERVOS[i][j].pin).us);
|
||||
}
|
||||
|
||||
// Forward kinematics, written independently of robot/Kinematics.cpp so it
|
||||
|
||||
@@ -18,7 +18,7 @@
|
||||
#endif
|
||||
|
||||
// Bump whenever a signature or struct below changes; the C# side checks it.
|
||||
#define HXSIM_API_VERSION 5
|
||||
#define HXSIM_API_VERSION 6
|
||||
|
||||
#define HXSIM_NUM_LEGS 6
|
||||
|
||||
@@ -34,8 +34,6 @@ typedef struct {
|
||||
float femur_min_deg, femur_max_deg;
|
||||
float tibia_min_deg, tibia_max_deg;
|
||||
float control_hz;
|
||||
int32_t servo_us_at_0deg;
|
||||
int32_t servo_us_per_deg;
|
||||
float leg_clearance_mm; // LEG_CLEARANCE_MM: minimum gap between neighbouring legs' segments
|
||||
float stance_height_range_mm; // CmdPacket.body_height +-1000 moves the height by this much
|
||||
} HxGeometry;
|
||||
@@ -81,7 +79,7 @@ HXSIM_API void hxsim_serial_input(const char* bytes, int32_t len); // TEST
|
||||
// Outputs
|
||||
HXSIM_API int32_t hxsim_serial_output(char* out, int32_t cap); // drains robot Serial prints
|
||||
HXSIM_API void hxsim_get_servos(int32_t* us_18, int32_t* attached_18, uint32_t* writes_18);
|
||||
HXSIM_API void hxsim_get_joints_deg(float* deg_18); // servo pulses decoded with Config.h constants
|
||||
HXSIM_API void hxsim_get_joints_deg(float* deg_18); // servo pulses decoded through Config.h's per-servo SERVOS table
|
||||
HXSIM_API void hxsim_get_feet_mm(float* xyz_18); // forward kinematics of those angles, body frame
|
||||
HXSIM_API void hxsim_get_status(HxStatus* out);
|
||||
// Kinematic body (SimBody.cpp): the body rests on the support plane of its
|
||||
|
||||
@@ -7,7 +7,7 @@ simulation is exactly the code you flash to the Teensy. `robot/` has no
|
||||
`#ifdef SIM` anywhere.
|
||||
|
||||
```
|
||||
robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 4.0 firmware
|
||||
robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 3.6 firmware
|
||||
└─► CMake + g++ + sim/shim/ ──► sim/build/hexapod_sim.dll ──► Unity
|
||||
```
|
||||
|
||||
@@ -17,7 +17,7 @@ robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 4.0 firmw
|
||||
|
||||
| Firmware uses | Simulated as |
|
||||
|---|---|
|
||||
| `Servo::writeMicroseconds()` | the pulse stored per pin, decoded back to angles with `Config.h`'s `SERVO_US_*` |
|
||||
| `analogWrite()` hardware PWM (how `ServoDriver` drives the servos) | the pulse width per pin, worked out from duty, resolution and frequency, then decoded back to angles through `Config.h`'s per-servo `SERVOS` table |
|
||||
| `Wire` + MPU6050 registers | a register-level fake MPU6050 at 0x68, fed from the simulated body's tilt and rotation |
|
||||
| `RF24` | a simulated radio: packets are received only if channel, address, data rate and **payload size** match, and the RX FIFO holds 3 |
|
||||
| `millis()` / `micros()` / `Serial` | a simulated clock that Unity advances, and captured serial I/O |
|
||||
@@ -114,7 +114,9 @@ component to the scene yourself.
|
||||
## What is not simulated
|
||||
|
||||
- Physics (mass, falling, slipping, servo torque and speed).
|
||||
- Servo calibration. The sim decodes pulses with `Config.h`'s single
|
||||
`SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG`, i.e. what the firmware *intends*,
|
||||
not per-servo offsets or directions.
|
||||
- Whether the calibration matches the real servos. The sim decodes pulses
|
||||
through the same `SERVOS` table the firmware uses, so it shows what the
|
||||
firmware *intends*, including pulse-limit clamping (which is how it
|
||||
caught the old ±90° coxa offsets). Whether a real servo turns that way is
|
||||
a bench check.
|
||||
- The remote firmware. `SimRemote.cs` mirrors its packet rules instead.
|
||||
@@ -4,6 +4,7 @@
|
||||
#include <Servo.h>
|
||||
#include <Wire.h>
|
||||
#include <RF24.h>
|
||||
#include <SPI.h>
|
||||
#include <EEPROM.h>
|
||||
#include <HexapodProtocol.h>
|
||||
#include <stdarg.h>
|
||||
@@ -13,6 +14,7 @@
|
||||
|
||||
SimSerial Serial;
|
||||
TwoWire Wire;
|
||||
SPIClass SPI;
|
||||
SimEEPROM EEPROM;
|
||||
|
||||
// =====================================================================
|
||||
@@ -103,6 +105,36 @@ void Servo::writeMicroseconds(int us) {
|
||||
g_pins[pin_].writes++;
|
||||
}
|
||||
|
||||
// ---- Hardware PWM (how ServoDriver drives the servos on the Teensy 3.6) ----
|
||||
static uint32_t g_pwm_bits = 8; // Teensyduino default
|
||||
static float g_pwm_hz[simhw::MAX_PINS]; // 0 = not set (Teensy default ~488 Hz)
|
||||
|
||||
void pinMode(uint8_t pin, uint8_t mode) {
|
||||
if (pin < simhw::MAX_PINS && mode == INPUT) g_pins[pin].attached = false;
|
||||
}
|
||||
void digitalWrite(uint8_t, uint8_t) {}
|
||||
uint32_t analogWriteResolution(uint32_t bits) {
|
||||
uint32_t old = g_pwm_bits;
|
||||
g_pwm_bits = bits;
|
||||
return old;
|
||||
}
|
||||
void analogWriteFrequency(uint8_t pin, float hz) {
|
||||
if (pin < simhw::MAX_PINS) g_pwm_hz[pin] = hz;
|
||||
}
|
||||
// A duty of 0 means no pulses (a relaxed servo); otherwise the pulse width
|
||||
// follows from duty, resolution and frequency, exactly as on the hardware.
|
||||
void analogWrite(uint8_t pin, int value) {
|
||||
if (pin >= simhw::MAX_PINS) return;
|
||||
if (value <= 0) {
|
||||
g_pins[pin].attached = false;
|
||||
return;
|
||||
}
|
||||
float hz = g_pwm_hz[pin] > 0 ? g_pwm_hz[pin] : 488.28f;
|
||||
g_pins[pin].us = (int)lroundf((float)value / (1u << g_pwm_bits) * (1e6f / hz));
|
||||
g_pins[pin].attached = true;
|
||||
g_pins[pin].writes++;
|
||||
}
|
||||
|
||||
// =====================================================================
|
||||
// I2C + MPU6050 (register-level, address 0x68)
|
||||
// =====================================================================
|
||||
|
||||
@@ -28,6 +28,18 @@ constexpr typename std::common_type<A, B>::type max(A a, B b) { return a > b ? a
|
||||
inline double radians(double deg) { return deg * (PI / 180.0); }
|
||||
inline double degrees(double rad) { return rad * (180.0 / PI); }
|
||||
|
||||
// Pins and hardware PWM. analogWrite() pulses are recorded per pin, so the
|
||||
// simulation sees servo pulses exactly as the PWM hardware would make them.
|
||||
#define INPUT 0
|
||||
#define OUTPUT 1
|
||||
#define LOW 0
|
||||
#define HIGH 1
|
||||
void pinMode(uint8_t pin, uint8_t mode);
|
||||
void digitalWrite(uint8_t pin, uint8_t value);
|
||||
void analogWrite(uint8_t pin, int value);
|
||||
uint32_t analogWriteResolution(uint32_t bits); // returns the previous resolution, like Teensyduino
|
||||
void analogWriteFrequency(uint8_t pin, float hz);
|
||||
|
||||
// Simulated clock, advanced by the simulation (see SimHardware.cpp).
|
||||
uint32_t millis();
|
||||
uint32_t micros();
|
||||
|
||||
@@ -1,2 +1,12 @@
|
||||
#pragma once
|
||||
// Nothing needed: the simulated RF24 doesn't talk SPI.
|
||||
#include <Arduino.h>
|
||||
|
||||
// The simulated RF24 doesn't talk SPI; this only has to accept the pin setup.
|
||||
class SPIClass {
|
||||
public:
|
||||
void begin() {}
|
||||
void setSCK(uint8_t) {}
|
||||
void setMOSI(uint8_t) {}
|
||||
void setMISO(uint8_t) {}
|
||||
};
|
||||
extern SPIClass SPI;
|
||||
Reference in new issue
Block a user