335 lines
9.9 KiB
C++
335 lines
9.9 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
|
|
// ============================================================================
|
|
|
|
void runNodeAnimation() {
|
|
static uint32_t lastTickTime = 0;
|
|
static uint32_t currentTick = 0;
|
|
static bool wasActive = false;
|
|
|
|
if (!animState.current || !animState.current->isActive()) {
|
|
wasActive = false;
|
|
return;
|
|
}
|
|
|
|
// Reset tick when animation starts or if currentTick is less than startFrame
|
|
if (!wasActive || currentTick < animState.startFrame) {
|
|
currentTick = animState.startFrame; // Start from specified frame
|
|
lastTickTime = millis();
|
|
wasActive = true;
|
|
// Debug: send startFrame via MSGE
|
|
sendMessage("Animation startFrame: " + String(animState.startFrame) + ", currentTick: " + String(currentTick));
|
|
}
|
|
|
|
config.enableAllMotors();
|
|
|
|
uint32_t now = millis();
|
|
if (now - lastTickTime < FRAME_INTERVAL_MS)
|
|
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");
|
|
}
|
|
|
|
// ============================================================================
|
|
// Main Loop
|
|
// ============================================================================
|
|
|
|
void loop() {
|
|
// Serial passthrough (when enabled)
|
|
#if ENABLE_SERIAL_PASSTHROUGH
|
|
handleSerialPassthrough();
|
|
return;
|
|
#endif
|
|
|
|
// Protocol handling
|
|
handleProtocol();
|
|
|
|
// Animation playback
|
|
runNodeAnimation();
|
|
|
|
// Motor position updates
|
|
updateMotorPositions();
|
|
handleMotorStreaming();
|
|
|
|
// Sensor updates and streaming
|
|
sensors.update();
|
|
|
|
// Heartbeat
|
|
sendHeartbeat();
|
|
}
|