diff --git a/HansonServo.ino b/HansonServo.ino index e23673d..61a545c 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -1,24 +1,29 @@ /** * HansonServo Firmware - * + * * Unified robot controller with: * - Feetech servo control (SCS/STS) * - Animation playback via node graphs - * - BNO055 IMU + * - ADXL345 IMU * - RD-03D mmWave radar * - CRC16 tagged packet protocol - * + * * Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B] */ -#include -#include "protocol.h" +// 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 "animation.h" #include "nodegraph.h" -#include "RobotConfig.h" +#include "protocol.h" #include "sensors.h" +#include + // ============================================================================ // Global State @@ -37,9 +42,9 @@ constexpr uint32_t HEARTBEAT_INTERVAL_MS = 1000; // ============================================================================ uint16_t getSineWaveValue(unsigned long centiseconds) { - constexpr uint16_t WAVE_PERIOD_CS = 400; // 4 seconds + 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); @@ -56,6 +61,24 @@ void handleProtocol() { } } +// ============================================================================ +// 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 // ============================================================================ @@ -80,28 +103,29 @@ void runNodeAnimation() { config.enableAllMotors(); uint32_t now = millis(); - if (now - lastTickTime < FRAME_INTERVAL_MS) return; - lastTickTime = now; + 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 motorIDs; - std::vector positions; - std::vector speeds; + std::vector motorIDs; + std::vector positions; + std::vector 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); + 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 { + } else { // Only disable torque for motors that should be limp - if (config.setMotorEnabled(motorID, false)) { + if (config.setMotorEnabled(motorID, false)) { servoManager[0]->disableTorque(motorID); } } @@ -109,14 +133,8 @@ void runNodeAnimation() { // 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 - ); + servoManager.syncWritePositions(motorIDs.data(), positions.data(), + speeds.data(), motorIDs.size(), config, 0); } // Emit per-frame event: [frameLo, frameHi, playMode, status=0] @@ -125,45 +143,45 @@ void runNodeAnimation() { payload[0] = currentTick & 0xFF; payload[1] = (currentTick >> 8) & 0xFF; payload[2] = static_cast(animState.playMode); - payload[3] = 0; // in-progress + payload[3] = 0; // in-progress sendPacket(Tag::FRAME, payload, 4); } - currentTick++; + currentTick++; // Handle animation end (0 = run indefinitely for variable-only animations) - if (animState.current->getFrameCount() > 0 && + if (animState.current->getFrameCount() > 0 && currentTick > animState.current->getFrameCount()) { 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(animState.playMode); - done[3] = 1; // complete - sendPacket(Tag::FRAME, done, 4); - } - break; - case PLAY_LOOP: - // Continue looping - 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(animState.playMode); - done[3] = 1; // complete - sendPacket(Tag::FRAME, done, 4); - } - break; - default: - break; + case PLAY_ONCE: + animState.stop(); + { + uint8_t done[4]; + done[0] = currentTick & 0xFF; + done[1] = (currentTick >> 8) & 0xFF; + done[2] = static_cast(animState.playMode); + done[3] = 1; // complete + sendPacket(Tag::FRAME, done, 4); } - currentTick = 0; + break; + case PLAY_LOOP: + // Continue looping + 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(animState.playMode); + done[3] = 1; // complete + sendPacket(Tag::FRAME, done, 4); + } + break; + default: + break; + } + currentTick = 0; } } @@ -174,10 +192,11 @@ void runNodeAnimation() { void updateMotorPositions() { static unsigned long lastUpdate = 0; - if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS) return; + if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS) + return; lastUpdate = millis(); - for (const Motor& motor : config.motors) { + 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); @@ -196,21 +215,27 @@ void handleMotorStreaming() { void sendHeartbeat() { static unsigned long lastHeartbeat = 0; - - if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS) return; + + 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; + 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; @@ -230,7 +255,7 @@ 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..."); @@ -265,19 +290,25 @@ void setup() { // ============================================================================ 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(); }