diff --git a/HansonServo.ino b/HansonServo.ino index dc852b4..b3f3f52 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -81,27 +81,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); + 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); } } @@ -119,27 +119,51 @@ void runNodeAnimation() { ); } - currentTick++; + // Emit per-frame event: [frameLo, frameHi, playMode, status=0] + { + uint8_t payload[4]; + payload[0] = currentTick & 0xFF; + payload[1] = (currentTick >> 8) & 0xFF; + payload[2] = static_cast(animState.playMode); + payload[3] = 0; // in-progress + sendPacket(Tag::FRAME, payload, 4); + } + + currentTick++; // Handle animation end (0 = run indefinitely for variable-only animations) if (animState.current->getFrameCount() > 0 && currentTick > animState.current->getFrameCount()) { switch (animState.playMode) { - case PLAY_ONCE: + case PLAY_ONCE: animState.stop(); - break; - case PLAY_LOOP: + { + 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: + break; + case PLAY_REPEAT: if (--animState.repeatsRemaining == 0) { animState.stop(); - } - break; + 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; + } + currentTick = 0; } } diff --git a/protocol.h b/protocol.h index 2cc9ca4..48338b1 100644 --- a/protocol.h +++ b/protocol.h @@ -43,6 +43,7 @@ namespace Tag { // Sensors constexpr char IMU[4] = {'I','M','U','0'}; // IMU data (heading, roll, pitch) constexpr char RADAR[4] = {'R','D','A','R'}; // Radar targets + constexpr char FRAME[4] = {'F','R','M','E'}; // Animation frame events (frame, mode, status) // System constexpr char STATE[4] = {'S','T','A','T'}; // System state/heartbeat diff --git a/sensors.cpp b/sensors.cpp index 3447310..3f7d3c0 100644 --- a/sensors.cpp +++ b/sensors.cpp @@ -178,8 +178,8 @@ bool IMU::update() { } uint16_t IMU::packPayload(uint8_t* buffer) const { - // Format: heading(2) + roll(2) + pitch(2) as int16 * 100 - int16_t h = (int16_t)(heading * 100.0f); + // Format: heading(2, unsigned) + roll(2, signed) + pitch(2, signed), all *100 + uint16_t h = (uint16_t)(heading * 100.0f); // 0..36000 fits in uint16 int16_t r = (int16_t)(roll * 100.0f); int16_t p = (int16_t)(pitch * 100.0f); diff --git a/sensors.h b/sensors.h index 275eca0..0d99d3f 100644 --- a/sensors.h +++ b/sensors.h @@ -106,10 +106,10 @@ public: bool isRadarStreamEnabled() const { return radarStreamEnabled; } private: - bool imuStreamEnabled = false; - bool radarStreamEnabled = false; - uint16_t imuInterval = 100; - uint16_t radarInterval = 100; + bool imuStreamEnabled = true; + bool radarStreamEnabled = true; + uint16_t imuInterval = 10; + uint16_t radarInterval = 10; unsigned long lastIMUSend = 0; unsigned long lastRadarSend = 0;