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

imu and current frame streaming

protocolv2
Jake 2025-12-11 11:01:17 +08:00
parent 7ddb756497
commit db0297cea9
4 changed files with 53 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
HansonServo.ino
View File

@ -81,27 +81,27 @@ void runNodeAnimation() {
uint32_t now = millis(); uint32_t now = millis();
if (now - lastTickTime < FRAME_INTERVAL_MS) return; if (now - lastTickTime < FRAME_INTERVAL_MS) return;
lastTickTime = now; lastTickTime = now;
// Tick the node graph // Tick the node graph
animState.current->nodeGraph.tick(currentTick, *animState.current); animState.current->nodeGraph.tick(currentTick, *animState.current);
auto outputs = animState.current->nodeGraph.getServoOutputs(); auto outputs = animState.current->nodeGraph.getServoOutputs();
// Collect motor commands // Collect motor commands
std::vector<uint8_t> motorIDs; std::vector<uint8_t> motorIDs;
std::vector<uint16_t> positions; std::vector<uint16_t> positions;
std::vector<uint16_t> speeds; std::vector<uint16_t> speeds;
for (const auto& [motorID, value] : outputs) { for (const auto& [motorID, value] : outputs) {
if (value != 65535) { if (value != 65535) {
motorIDs.push_back(motorID); motorIDs.push_back(motorID);
positions.push_back(value); positions.push_back(value);
speeds.push_back(0); speeds.push_back(0);
config.setMotorPosition(motorID, value); config.setMotorPosition(motorID, value);
config.setMotorEnabled(motorID, true); config.setMotorEnabled(motorID, true);
} else { } else {
// Only disable torque for motors that should be limp // Only disable torque for motors that should be limp
if (config.setMotorEnabled(motorID, false)) { if (config.setMotorEnabled(motorID, false)) {
servoManager[0]->disableTorque(motorID); 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<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) // Handle animation end (0 = run indefinitely for variable-only animations)
if (animState.current->getFrameCount() > 0 && if (animState.current->getFrameCount() > 0 &&
currentTick > animState.current->getFrameCount()) { currentTick > animState.current->getFrameCount()) {
switch (animState.playMode) { switch (animState.playMode) {
case PLAY_ONCE: case PLAY_ONCE:
animState.stop(); animState.stop();
break; {
case PLAY_LOOP: 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 // Continue looping
break; break;
case PLAY_REPEAT: case PLAY_REPEAT:
if (--animState.repeatsRemaining == 0) { if (--animState.repeatsRemaining == 0) {
animState.stop(); animState.stop();
} uint8_t done[4];
break; 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;
default: default:
break; break;
} }
currentTick = 0; currentTick = 0;
} }
} }

1
protocol.h
View File

@ -43,6 +43,7 @@ namespace Tag {
// Sensors // Sensors
constexpr char IMU[4] = {'I','M','U','0'}; // IMU data (heading, roll, pitch) 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 RADAR[4] = {'R','D','A','R'}; // Radar targets
constexpr char FRAME[4] = {'F','R','M','E'}; // Animation frame events (frame, mode, status)
// System // System
constexpr char STATE[4] = {'S','T','A','T'}; // System state/heartbeat constexpr char STATE[4] = {'S','T','A','T'}; // System state/heartbeat

4
sensors.cpp
View File

@ -178,8 +178,8 @@ bool IMU::update() {
} }
uint16_t IMU::packPayload(uint8_t* buffer) const { uint16_t IMU::packPayload(uint8_t* buffer) const {
// Format: heading(2) + roll(2) + pitch(2) as int16 * 100 // Format: heading(2, unsigned) + roll(2, signed) + pitch(2, signed), all *100
int16_t h = (int16_t)(heading * 100.0f); uint16_t h = (uint16_t)(heading * 100.0f); // 0..36000 fits in uint16
int16_t r = (int16_t)(roll * 100.0f); int16_t r = (int16_t)(roll * 100.0f);
int16_t p = (int16_t)(pitch * 100.0f); int16_t p = (int16_t)(pitch * 100.0f);

8
sensors.h
View File

@ -106,10 +106,10 @@ public:
bool isRadarStreamEnabled() const { return radarStreamEnabled; } bool isRadarStreamEnabled() const { return radarStreamEnabled; }
private: private:
bool imuStreamEnabled = false; bool imuStreamEnabled = true;
bool radarStreamEnabled = false; bool radarStreamEnabled = true;
uint16_t imuInterval = 100; uint16_t imuInterval = 10;
uint16_t radarInterval = 100; uint16_t radarInterval = 10;
unsigned long lastIMUSend = 0; unsigned long lastIMUSend = 0;
unsigned long lastRadarSend = 0; unsigned long lastRadarSend = 0;