realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added play from and stop commands

protocolv2
Jake 2025-12-15 10:46:37 +08:00
parent d603ef2e18
commit 77a151ad95
6 changed files with 97 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
HansonServo.ino
View File

@ -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<uint8_t> motorIDs;
std::vector<uint16_t> positions;
std::vector<uint16_t> speeds;
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);
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<uint8_t>(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<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];
@ -176,12 +192,16 @@ void runNodeAnimation() {
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;
}
currentTick = 0;
// Don't reset currentTick here - each case handles it if needed
}
}

15
PROTOCOL_MIGRATION.md
View File

@ -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

28
commands.cpp
View File

@ -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<PlayMode>(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
// ============================================================================

4
commands.h
View File

@ -15,8 +15,9 @@ struct AnimationState {
Animation* current = nullptr;
PlayMode playMode = PLAY_IDLE;
uint8_t repeatsRemaining = 0;
uint16_t startFrame = 0; // Frame to start playback from
void play(PlayMode mode, uint8_t repeats = 0);
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);

1
protocol.h
View File

@ -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

2
sensors.h
View File

@ -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;