502 lines
16 KiB
C++
502 lines
16 KiB
C++
/**
|
|
* HansonServo Firmware
|
|
*
|
|
* Unified robot controller with:
|
|
* - Feetech servo control (SCS/STS)
|
|
* - Animation playback via node graphs
|
|
* - ADXL345 IMU
|
|
* - RD-03D mmWave radar
|
|
* - CRC16 tagged packet protocol
|
|
*
|
|
* Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B]
|
|
*/
|
|
|
|
// Configuration defines
|
|
#define ENABLE_SERIAL_PASSTHROUGH \
|
|
false // Set true to use Feetech app comms straight to servos
|
|
|
|
#include "robotconfig.h"
|
|
#include "animation.h"
|
|
#include "commands.h"
|
|
#include "motorcontrol.h"
|
|
#include "nodegraph.h"
|
|
#include "protocol.h"
|
|
#include "sensors.h"
|
|
#include <FFat.h>
|
|
|
|
|
|
// ============================================================================
|
|
// Global State
|
|
// ============================================================================
|
|
|
|
RobotConfig config;
|
|
|
|
// Timing constants
|
|
constexpr uint16_t FRAME_RATE = 48;
|
|
constexpr uint16_t FRAME_INTERVAL_MS = 1000 / FRAME_RATE;
|
|
constexpr uint16_t MOTOR_UPDATE_INTERVAL_MS = 50;
|
|
constexpr uint32_t HEARTBEAT_INTERVAL_MS = 1000;
|
|
|
|
// ============================================================================
|
|
// Utility Functions
|
|
// ============================================================================
|
|
|
|
uint16_t getSineWaveValue(unsigned long centiseconds) {
|
|
constexpr uint16_t WAVE_PERIOD_CS = 400; // 4 seconds
|
|
constexpr uint16_t WAVE_MAX = 4095;
|
|
|
|
float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
|
|
float sine = sin(theta);
|
|
float scaled = (sine + 1.0) * (WAVE_MAX / 2.0);
|
|
return (uint16_t)round(scaled);
|
|
}
|
|
|
|
// ============================================================================
|
|
// Protocol Handler
|
|
// ============================================================================
|
|
|
|
void handleProtocol() {
|
|
if (receivePacket()) {
|
|
dispatchCommand();
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Serial Passthrough (USB ↔ Servo Serial)
|
|
// ============================================================================
|
|
|
|
#if ENABLE_SERIAL_PASSTHROUGH
|
|
void handleSerialPassthrough() {
|
|
// Forward USB Serial → Servo Serial1
|
|
while (Serial.available()) {
|
|
Serial1.write(Serial.read());
|
|
}
|
|
|
|
// Forward Servo Serial1 → USB Serial
|
|
while (Serial1.available()) {
|
|
Serial.write(Serial1.read());
|
|
}
|
|
}
|
|
#endif
|
|
|
|
// ============================================================================
|
|
// Animation Playback
|
|
// ============================================================================
|
|
|
|
// Dispatcher: calls the appropriate animation function based on version
|
|
void runAnimation() {
|
|
if (!animState.current || !animState.current->isActive()) {
|
|
return;
|
|
}
|
|
|
|
if (animState.current->header.version == 2) {
|
|
runFrameAnimation();
|
|
} else {
|
|
runNodeAnimation();
|
|
}
|
|
}
|
|
|
|
// Version 2: Frame-by-frame animation playback
|
|
void runFrameAnimation() {
|
|
static uint32_t lastTickTime = 0;
|
|
static uint32_t currentTick = 0;
|
|
static uint8_t lastGeneration = 0;
|
|
|
|
if (!animState.current || !animState.current->isActive()) {
|
|
return;
|
|
}
|
|
|
|
// Reset tick when a new animation starts (detected by generation change)
|
|
if (lastGeneration != animState.playGeneration) {
|
|
currentTick = animState.startFrame;
|
|
lastTickTime = millis();
|
|
lastGeneration = animState.playGeneration;
|
|
sendMessage("V2 Animation started, generation: " + String(lastGeneration) + ", startFrame: " + String(animState.startFrame));
|
|
}
|
|
|
|
config.enableAllMotors();
|
|
|
|
// Calculate frame interval from animation's frame rate
|
|
uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
|
|
if (frameIntervalMs == 0) frameIntervalMs = 1; // Safety: prevent division by zero
|
|
|
|
uint32_t now = millis();
|
|
if (now - lastTickTime < frameIntervalMs)
|
|
return;
|
|
lastTickTime = now;
|
|
|
|
// Get frame data for current tick
|
|
const std::vector<MotorPosition>* frameData = animState.current->getFrameData(currentTick);
|
|
|
|
if (frameData && !frameData->empty()) {
|
|
// Collect motor commands from frame data
|
|
std::vector<uint8_t> motorIDs;
|
|
std::vector<uint16_t> positions;
|
|
std::vector<uint16_t> speeds;
|
|
|
|
for (const auto& motorPos : *frameData) {
|
|
motorIDs.push_back(motorPos.motorID);
|
|
positions.push_back(motorPos.position);
|
|
speeds.push_back(0);
|
|
config.setMotorPosition(motorPos.motorID, motorPos.position);
|
|
config.setMotorEnabled(motorPos.motorID, true);
|
|
}
|
|
|
|
// Send all positions in one sync write
|
|
if (!motorIDs.empty()) {
|
|
servoManager.syncWritePositions(motorIDs.data(), positions.data(),
|
|
speeds.data(), motorIDs.size(), config, 0);
|
|
}
|
|
|
|
// Debug: print frame and motor positions
|
|
// Serial.print("Frame ");
|
|
// Serial.print(currentTick);
|
|
// Serial.print(": ");
|
|
// for (size_t i = 0; i < motorIDs.size(); i++) {
|
|
// if (i > 0) Serial.print(", ");
|
|
// Serial.print("M");
|
|
// Serial.print(motorIDs[i]);
|
|
// Serial.print("=");
|
|
// Serial.print(positions[i]);
|
|
// }
|
|
// Serial.println();
|
|
}
|
|
|
|
// Emit per-frame event
|
|
{
|
|
uint8_t payload[4];
|
|
payload[0] = currentTick & 0xFF;
|
|
payload[1] = (currentTick >> 8) & 0xFF;
|
|
payload[2] = static_cast<uint8_t>(animState.playMode);
|
|
payload[3] = 0; // in-progress
|
|
sendPacket(Tag::FRAME, payload, 4);
|
|
}
|
|
|
|
currentTick++;
|
|
|
|
// Handle animation end
|
|
uint16_t totalFrames = animState.current->getFrameCount();
|
|
uint16_t framesPlayed = currentTick - animState.startFrame;
|
|
uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;
|
|
|
|
// Debug: log completion check near the end
|
|
if (remainingFrames > 0 && framesPlayed >= remainingFrames - 1) {
|
|
sendMessage("Completion check - currentTick: " + String(currentTick) + ", framesPlayed: " + String(framesPlayed) + ", remainingFrames: " + String(remainingFrames) + ", totalFrames: " + String(totalFrames));
|
|
}
|
|
|
|
// Check if we've played all remaining frames (from startFrame to totalFrames-1)
|
|
if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
|
|
sendMessage("Animation completion triggered! Sending completion packet.");
|
|
switch (animState.playMode) {
|
|
case PLAY_ONCE:
|
|
{
|
|
// Send completion packet BEFORE stopping animation
|
|
uint8_t done[4];
|
|
done[0] = currentTick & 0xFF;
|
|
done[1] = (currentTick >> 8) & 0xFF;
|
|
done[2] = static_cast<uint8_t>(animState.playMode);
|
|
done[3] = 1; // complete
|
|
sendPacket(Tag::FRAME, done, 4);
|
|
animState.stop();
|
|
}
|
|
break;
|
|
case PLAY_LOOP:
|
|
// Reset to start frame for seamless looping
|
|
currentTick = animState.startFrame;
|
|
break;
|
|
case PLAY_REPEAT:
|
|
if (--animState.repeatsRemaining == 0) {
|
|
// Send completion packet BEFORE stopping animation
|
|
uint8_t done[4];
|
|
done[0] = currentTick & 0xFF;
|
|
done[1] = (currentTick >> 8) & 0xFF;
|
|
done[2] = static_cast<uint8_t>(animState.playMode);
|
|
done[3] = 1; // complete
|
|
sendPacket(Tag::FRAME, done, 4);
|
|
animState.stop();
|
|
} else {
|
|
currentTick = animState.startFrame;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Version 1: Node graph animation playback
|
|
void runNodeAnimation() {
|
|
static uint32_t lastTickTime = 0;
|
|
static uint32_t currentTick = 0;
|
|
static uint8_t lastGeneration = 0;
|
|
|
|
if (!animState.current || !animState.current->isActive()) {
|
|
return;
|
|
}
|
|
|
|
// Reset tick when a new animation starts (detected by generation change)
|
|
if (lastGeneration != animState.playGeneration) {
|
|
currentTick = animState.startFrame; // Start from specified frame
|
|
lastTickTime = millis();
|
|
lastGeneration = animState.playGeneration;
|
|
// Debug: send startFrame via MSGE
|
|
sendMessage("Animation startFrame: " + String(animState.startFrame) + ", currentTick: " + String(currentTick));
|
|
}
|
|
|
|
config.enableAllMotors();
|
|
|
|
// Calculate frame interval from animation's frame rate
|
|
uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
|
|
if (frameIntervalMs == 0) frameIntervalMs = 1; // Safety: prevent division by zero
|
|
|
|
uint32_t now = millis();
|
|
if (now - lastTickTime < frameIntervalMs)
|
|
return;
|
|
lastTickTime = now;
|
|
|
|
// Tick the node graph
|
|
animState.current->nodeGraph.tick(currentTick, *animState.current);
|
|
auto outputs = animState.current->nodeGraph.getServoOutputs();
|
|
|
|
// Collect motor commands
|
|
std::vector<uint8_t> motorIDs;
|
|
std::vector<uint16_t> positions;
|
|
std::vector<uint16_t> speeds;
|
|
|
|
for (const auto &[motorID, value] : outputs) {
|
|
if (value != 65535) {
|
|
motorIDs.push_back(motorID);
|
|
positions.push_back(value);
|
|
speeds.push_back(0);
|
|
config.setMotorPosition(motorID, value);
|
|
config.setMotorEnabled(motorID, true);
|
|
} else {
|
|
// Only disable torque for motors that should be limp
|
|
if (config.setMotorEnabled(motorID, false)) {
|
|
servoManager[0]->disableTorque(motorID);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Send all positions in one sync write - motors move together!
|
|
if (!motorIDs.empty()) {
|
|
servoManager.syncWritePositions(motorIDs.data(), positions.data(),
|
|
speeds.data(), motorIDs.size(), config, 0);
|
|
}
|
|
|
|
// Emit per-frame event: [frameLo, frameHi, playMode, status=0]
|
|
// Send actual frame number (currentTick), not relative frame
|
|
{
|
|
uint8_t payload[4];
|
|
payload[0] = currentTick & 0xFF;
|
|
payload[1] = (currentTick >> 8) & 0xFF;
|
|
payload[2] = static_cast<uint8_t>(animState.playMode);
|
|
payload[3] = 0; // in-progress
|
|
sendPacket(Tag::FRAME, payload, 4);
|
|
}
|
|
|
|
currentTick++;
|
|
|
|
// Handle animation end (0 = run indefinitely for variable-only animations)
|
|
// Calculate total frames played: currentTick - startFrame
|
|
uint16_t framesPlayed = currentTick - animState.startFrame;
|
|
// Calculate remaining frames: total frames minus startFrame
|
|
// If animation has 100 frames and we start at 50, we should only play 50 frames
|
|
uint16_t totalFrames = animState.current->getFrameCount();
|
|
uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;
|
|
|
|
// Debug: show completion check values every 10 frames
|
|
if (framesPlayed % 10 == 0 || framesPlayed >= remainingFrames - 1) {
|
|
sendMessage("Frame check - played: " + String(framesPlayed) + ", remaining: " + String(remainingFrames) + ", total: " + String(totalFrames) + ", startFrame: " + String(animState.startFrame));
|
|
}
|
|
|
|
if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
|
|
switch (animState.playMode) {
|
|
case PLAY_ONCE:
|
|
animState.stop();
|
|
{
|
|
uint8_t done[4];
|
|
done[0] = currentTick & 0xFF;
|
|
done[1] = (currentTick >> 8) & 0xFF;
|
|
done[2] = static_cast<uint8_t>(animState.playMode);
|
|
done[3] = 1; // complete
|
|
sendPacket(Tag::FRAME, done, 4);
|
|
}
|
|
break;
|
|
case PLAY_LOOP:
|
|
// Reset to start frame for seamless looping
|
|
currentTick = animState.startFrame;
|
|
sendMessage("Looping back to startFrame: " + String(animState.startFrame));
|
|
break;
|
|
case PLAY_REPEAT:
|
|
if (--animState.repeatsRemaining == 0) {
|
|
animState.stop();
|
|
uint8_t done[4];
|
|
done[0] = currentTick & 0xFF;
|
|
done[1] = (currentTick >> 8) & 0xFF;
|
|
done[2] = static_cast<uint8_t>(animState.playMode);
|
|
done[3] = 1; // complete
|
|
sendPacket(Tag::FRAME, done, 4);
|
|
// Animation stopped, don't reset tick
|
|
} else {
|
|
// Reset to start frame for next repeat
|
|
currentTick = animState.startFrame;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
// Don't reset currentTick here - each case handles it if needed
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Motor Position Updates
|
|
// ============================================================================
|
|
|
|
void updateMotorPositions() {
|
|
static unsigned long lastUpdate = 0;
|
|
|
|
if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
|
|
return;
|
|
lastUpdate = millis();
|
|
|
|
for (const Motor &motor : config.motors) {
|
|
servoManager[0]->setFeetechMode(motor.servoModel.major);
|
|
uint16_t position = servoManager[0]->getPosition(motor.motorID);
|
|
config.setMotorPosition(motor.motorID, position);
|
|
}
|
|
}
|
|
|
|
void handleMotorStreaming() {
|
|
if (motorStream.shouldStream()) {
|
|
sendMotorPositions();
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Heartbeat
|
|
// ============================================================================
|
|
|
|
void sendHeartbeat() {
|
|
static unsigned long lastHeartbeat = 0;
|
|
|
|
if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS)
|
|
return;
|
|
lastHeartbeat = millis();
|
|
|
|
// Pack state: uptime(4) + flags(2)
|
|
uint8_t payload[6];
|
|
uint32_t uptime = millis() / 1000;
|
|
uint16_t flags = 0;
|
|
|
|
// Build flags
|
|
if (adxl.isReady())
|
|
flags |= 0x01;
|
|
if (animState.current && animState.current->isActive())
|
|
flags |= 0x02;
|
|
if (motorStream.active)
|
|
flags |= 0x04;
|
|
if (sensors.isADXLStreamEnabled())
|
|
flags |= 0x08;
|
|
if (sensors.isRadarStreamEnabled())
|
|
flags |= 0x10;
|
|
|
|
payload[0] = uptime & 0xFF;
|
|
payload[1] = (uptime >> 8) & 0xFF;
|
|
payload[2] = (uptime >> 16) & 0xFF;
|
|
payload[3] = (uptime >> 24) & 0xFF;
|
|
payload[4] = flags & 0xFF;
|
|
payload[5] = (flags >> 8) & 0xFF;
|
|
|
|
sendPacket(Tag::STATE, payload, 6);
|
|
}
|
|
|
|
// ============================================================================
|
|
// Setup
|
|
// ============================================================================
|
|
|
|
void setup() {
|
|
// Serial setup (buffer size must be set before begin)
|
|
Serial.setRxBufferSize(8192);
|
|
Serial.begin(1000000);
|
|
|
|
// Startup delay
|
|
delay(500);
|
|
Serial.println("\n[HansonServo] Starting...");
|
|
|
|
// Initialize servo manager
|
|
servoManager.init();
|
|
Serial.println("[HansonServo] Servos initialized");
|
|
|
|
// Initialize sensors
|
|
sensors.init();
|
|
|
|
// Initialize filesystem
|
|
if (!FFat.begin(true)) {
|
|
Serial.println("[HansonServo] FFat mount failed!");
|
|
return;
|
|
}
|
|
Serial.println("[HansonServo] Filesystem ready");
|
|
|
|
|
|
|
|
// Load or create robot config
|
|
if (config.loadOrCreateDefault()) {
|
|
Serial.println("[HansonServo] Config loaded: " + config.deviceName);
|
|
} else {
|
|
Serial.println("[HansonServo] Config init failed");
|
|
}
|
|
|
|
Serial.println("[HansonServo] Ready");
|
|
Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
|
|
|
|
// ---- TEST: Load and play animation ----
|
|
// Serial.println("[TEST] Loading /slow.anim...");
|
|
// if (animState.animation.loadFromFile("/slow.anim")) {
|
|
// Serial.println("[TEST] Animation loaded successfully");
|
|
|
|
// delay(1000); // Wait 1 second
|
|
|
|
// // Print animation info
|
|
// Serial.println(animState.animation.printAnim());
|
|
|
|
// delay(5000); // Wait 5 seconds
|
|
|
|
// // Play the animation
|
|
// Serial.println("[TEST] Playing animation...");
|
|
// animState.play(PLAY_ONCE, 1, 0);
|
|
// } else {
|
|
// Serial.println("[TEST] Failed to load /animation.anim");
|
|
// }
|
|
// ---- END TEST ----
|
|
}
|
|
|
|
// ============================================================================
|
|
// Main Loop
|
|
// ============================================================================
|
|
|
|
void loop() {
|
|
// Serial passthrough (when enabled)
|
|
#if ENABLE_SERIAL_PASSTHROUGH
|
|
handleSerialPassthrough();
|
|
return;
|
|
#endif
|
|
|
|
// Protocol handling
|
|
handleProtocol();
|
|
|
|
// Animation playback (auto-selects v1 node or v2 frame based on version)
|
|
runAnimation();
|
|
|
|
// Motor position updates
|
|
updateMotorPositions();
|
|
handleMotorStreaming();
|
|
|
|
// Sensor updates and streaming
|
|
//sensors.update();
|
|
|
|
// Heartbeat
|
|
sendHeartbeat();
|
|
}
|