diff --git a/HansonServo.ino b/HansonServo.ino index 61a545c..4a52075 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -93,11 +93,13 @@ void runNodeAnimation() { return; } - // Reset tick when animation starts - if (!wasActive) { - currentTick = 0; + // 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(); @@ -105,27 +107,27 @@ void runNodeAnimation() { uint32_t now = millis(); if (now - lastTickTime < FRAME_INTERVAL_MS) return; - lastTickTime = now; + 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); + 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); } } @@ -138,6 +140,7 @@ void runNodeAnimation() { } // 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; @@ -147,27 +150,40 @@ void runNodeAnimation() { sendPacket(Tag::FRAME, payload, 4); } - currentTick++; + currentTick++; // Handle animation end (0 = run indefinitely for variable-only animations) - if (animState.current->getFrameCount() > 0 && - currentTick > animState.current->getFrameCount()) { + // 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: + 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: + { + 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: + // 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]; @@ -176,12 +192,16 @@ void runNodeAnimation() { done[2] = static_cast(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; } - currentTick = 0; + // Don't reset currentTick here - each case handles it if needed } } diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index d9b3dd3..ebdea12 100644 --- a/PROTOCOL_MIGRATION.md +++ b/PROTOCOL_MIGRATION.md @@ -91,6 +91,7 @@ ushort Crc16Ccitt(byte[] data) | CMD_MESSAGE | 0x06 | `MSGE` | Log/debug message | | CMD_SET_POSITION | 0x07 | `MSET` | Set motor positions | | CMD_PLAY_FILE | 0x08 | `FPLY` | Play animation | +| CMD_STOP_FILE | 0x09 | `FSTP` | Stop animation | | CMD_SCAN_CHANNEL | 0x09 | `MSCN` | Scan for motors | | CMD_WRITE_DATA | 0x10 | `MWRT` | Write motor register | | CMD_WRITE_CONFIG_UPDATE | 0x12 | `CONF` | Update config | @@ -160,9 +161,23 @@ ushort Crc16Ccitt(byte[] data) [filename: N bytes] [play_mode: 1 byte] // 0=idle, 1=once, 2=loop, 3=repeat [repeat_count: 1 byte] +[start_frame: 2 bytes LE] // Frame number to start playback from (0-based) ``` **Response:** `ACK!` on success, `NACK` if file not found +**Notes:** +- `start_frame` allows resuming playback from a specific frame +- FRAME packets report actual frame numbers (i.e., if start_frame=163, FRAME packets will show 163, 164, 165...) + +#### `FSTP` - Stop Animation +**Request:** Empty payload (0 bytes) +**Response:** `ACK!` on success + +**Notes:** +- Immediately stops the currently playing animation regardless of play mode +- Motors remain in their current positions (torque not disabled) +- No FRAME completion packet is sent + --- ### Motor Control diff --git a/commands.cpp b/commands.cpp index 45cd28c..6d112c9 100644 --- a/commands.cpp +++ b/commands.cpp @@ -42,11 +42,12 @@ static void resetSaveSession() { // AnimationState // ============================================================================ -void AnimationState::play(PlayMode mode, uint8_t repeats) { +void AnimationState::play(PlayMode mode, uint8_t repeats, uint16_t startFrame) { current = &animation; current->setActive(true); playMode = mode; repeatsRemaining = repeats; + this->startFrame = startFrame; } void AnimationState::stop() { @@ -110,6 +111,9 @@ void dispatchCommand() { else if (tagMatches(tag, Tag::FPLAY)) { handleFilePlay(payload, len); } + else if (tagMatches(tag, Tag::FSTP)) { + handleFileStop(payload, len); + } // Motor Control else if (tagMatches(tag, Tag::MSET)) { handleMotorSet(payload, len); @@ -370,14 +374,14 @@ void handleFileDelete(const uint8_t* payload, uint16_t len) { } void handleFilePlay(const uint8_t* payload, uint16_t len) { - if (len < 4) { + if (len < 6) { // Minimum: filenameLen(2) + filename(1) + mode(1) + repeats(1) + startFrame(2) sendNack(Tag::FPLAY, "Invalid request"); return; } - + uint16_t filenameLen = payload[0] | (payload[1] << 8); - if (len < 2 + filenameLen + 2) { - sendNack(Tag::FPLAY, "Invalid filename"); + if (len < 2 + filenameLen + 4) { // filenameLen + filename + mode + repeats + startFrame + sendNack(Tag::FPLAY, "Invalid payload length"); return; } @@ -388,19 +392,31 @@ void handleFilePlay(const uint8_t* payload, uint16_t len) { uint16_t offset = 2 + filenameLen; PlayMode mode = static_cast(payload[offset]); uint8_t repeats = payload[offset + 1]; + uint16_t startFrame = payload[offset + 2] | (payload[offset + 3] << 8); + + // Debug: show parsed startFrame + sendMessage("FPLAY parsed - startFrame bytes: [" + String(payload[offset + 2]) + ", " + String(payload[offset + 3]) + "] = " + String(startFrame)); animState.animation.clear(); String fullPath = "/" + String(filename); - + if (animState.animation.loadFromFile(fullPath.c_str())) { - animState.play(mode, repeats); + animState.play(mode, repeats, startFrame); sendAck(Tag::FPLAY); - sendMessage("Playing: " + String(filename)); + sendMessage("Playing: " + String(filename) + " from frame " + String(startFrame)); + sendMessage("animState.startFrame stored as: " + String(animState.startFrame)); } else { sendNack(Tag::FPLAY, "Load failed"); } } +void handleFileStop(const uint8_t* payload, uint16_t len) { + // FSTP has no payload (len should be 0, but we don't strictly require it) + animState.stop(); + sendAck(Tag::FSTP); + sendMessage("Animation stopped"); +} + // ============================================================================ // Motor Control Handlers // ============================================================================ diff --git a/commands.h b/commands.h index 405517a..6d61776 100644 --- a/commands.h +++ b/commands.h @@ -15,8 +15,9 @@ struct AnimationState { Animation* current = nullptr; PlayMode playMode = PLAY_IDLE; uint8_t repeatsRemaining = 0; - - void play(PlayMode mode, uint8_t repeats = 0); + uint16_t startFrame = 0; // Frame to start playback from + + void play(PlayMode mode, uint8_t repeats = 0, uint16_t startFrame = 0); void stop(); }; @@ -59,6 +60,7 @@ void handleFileLoad(const uint8_t* payload, uint16_t len); void handleFileSave(const uint8_t* payload, uint16_t len); void handleFileDelete(const uint8_t* payload, uint16_t len); void handleFilePlay(const uint8_t* payload, uint16_t len); +void handleFileStop(const uint8_t* payload, uint16_t len); // Motor Control void handleMotorSet(const uint8_t* payload, uint16_t len); diff --git a/protocol.h b/protocol.h index c5aed65..c31b3e1 100644 --- a/protocol.h +++ b/protocol.h @@ -32,6 +32,7 @@ namespace Tag { constexpr char FSAVE[4] = {'F','S','A','V'}; // File save constexpr char FDELE[4] = {'F','D','E','L'}; // File delete constexpr char FPLAY[4] = {'F','P','L','Y'}; // Play animation file + constexpr char FSTP[4] = {'F','S','T','P'}; // Stop animation // Motor Control constexpr char MSET[4] = {'M','S','E','T'}; // Set motor positions diff --git a/sensors.h b/sensors.h index d45fdbf..50232c5 100644 --- a/sensors.h +++ b/sensors.h @@ -115,7 +115,7 @@ public: private: bool adxlStreamEnabled = true; - bool radarStreamEnabled = false; + bool radarStreamEnabled = true; uint16_t adxlInterval = 10; uint16_t radarInterval = 10; unsigned long lastADXLSend = 0;