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

variable, math, and map() nodes are implemented

master
Jake 2025-10-18 23:21:24 +08:00
parent 29758d8abf
commit eaedb1ac3f
5 changed files with 517 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

204
HansonServo.ino
View File

@ -38,8 +38,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
#define RE_PIN 8 // Receiver Enable #define RE_PIN 8 // Receiver Enable
Animation anim; Animation anim;
Animation sweep; Animation* currentAnimation;
Animation stagger;
bool streamPositions = false; bool streamPositions = false;
unsigned long lastStreamPositions = 0; unsigned long lastStreamPositions = 0;
@ -59,45 +58,15 @@ uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 };
uint16_t pos1STS[] = { 0, 0 }; uint16_t pos1STS[] = { 0, 0 };
uint16_t pos2STS[] = { 1023, 1023 }; uint16_t pos2STS[] = { 1023, 1023 };
const uint8_t testPayload[] = { #define WAVE_PERIOD_CS 400 // 4 seconds = 400 centiseconds
// 🔹 Filename block (length = 17) #define WAVE_MAX 4095
17, 0,
'/', 'p', 'o', 'i', 'n', 't', 'c', 'u', 'r', 'v', 'e', '2', '.', 'a', 'n', 'i', 'm',
// 🔹 Header: "ANIM", frameCount=800, version=1, frameRate=50, reserved[8] uint16_t getSineWaveValue(unsigned long centiseconds) {
'A', 'N', 'I', 'M', float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
32, 3, // frameCount = 800 (little-endian) float sine = sin(theta); // ranges from -1 to +1
1, // version float scaled = (sine + 1.0) * (WAVE_MAX / 2.0); // scale to 0–4095
50, // frameRate return (uint16_t)round(scaled);
0, 0, 0, 0, 0, 0, 0, 0, // reserved }
// 🔹 Curve count = 2
2, 0,
// 🔹 Curve Segment 1
10, // motorID
0, 0, // startTime
40, 0, // endTime
44, 1, // startPointY = 300
10, 0, // startHandleX
44, 1, // startHandleY = 300
30, 0, // endHandleX
0, 0, // endHandleY
0, 0, // endPointY
// 🔹 Curve Segment 2
10, // motorID
40, 0, // startTime
32, 3, // endTime = 800
0, 0, // startPointY
200, 0, // startHandleX
0, 0, // startHandleY
88, 2, // endHandleX = 600
44, 1, // endHandleY = 300
44, 1 // endPointY = 300
};
const uint16_t testPayloadLength = sizeof(testPayload);
void setup() { void setup() {
@ -108,6 +77,9 @@ void setup() {
delay(500); delay(500);
} }
pinMode(6, INPUT);
pinMode(7, INPUT);
servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN); // SCS servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN); // SCS
servos[0]->setFeetechMode(Feetech::MODE_SCS); servos[0]->setFeetechMode(Feetech::MODE_SCS);
servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN); // STS servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN); // STS
@ -659,31 +631,35 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
for (uint16_t i = 0; i < curveCount; i++) { for (uint16_t i = 0; i < curveCount; i++) {
CurveSegment seg; CurveSegment seg;
memcpy(&seg, ptr, sizeof(CurveSegment)); memcpy(&seg, ptr, sizeof(CurveSegment));
Serial.print("Segment "); // Serial.print("Segment ");
Serial.print(i); // Serial.print(i);
Serial.print(": motorID="); // Serial.print(": motorID=");
Serial.print(seg.motorID); // Serial.print(seg.motorID);
Serial.print(", startTime="); // Serial.print(", startTime=");
Serial.print(seg.startTime); // Serial.print(seg.startTime);
Serial.print(", endTime="); // Serial.print(", endTime=");
Serial.print(seg.endTime); // Serial.print(seg.endTime);
Serial.print(", startPointY="); // Serial.print(", startPointY=");
Serial.print(seg.startPointY); // Serial.print(seg.startPointY);
Serial.print(", startHandleX="); // Serial.print(", startHandleX=");
Serial.print(seg.startHandleX); // Serial.print(seg.startHandleX);
Serial.print(", startHandleY="); // Serial.print(", startHandleY=");
Serial.print(seg.startHandleY); // Serial.print(seg.startHandleY);
Serial.print(", endHandleX="); // Serial.print(", endHandleX=");
Serial.print(seg.endHandleX); // Serial.print(seg.endHandleX);
Serial.print(", endHandleY="); // Serial.print(", endHandleY=");
Serial.print(seg.endHandleY); // Serial.print(seg.endHandleY);
Serial.print(", endPointY="); // Serial.print(", endPointY=");
Serial.println(seg.endPointY); // Serial.println(seg.endPointY);
animation.addCurveSegment(seg); animation.addCurveSegment(seg);
ptr += sizeof(CurveSegment); ptr += sizeof(CurveSegment);
} }
sendMessage(anim.printCurves());
// for (int i = 0; i < 800; i++){
// sendMessage(String(anim.getMotorPosition(11, i)));
// }
// ✅ Advance ptr to node graph payload // ✅ Advance ptr to node graph payload
//ptr += curveCount * sizeof(CurveSegment); //ptr += curveCount * sizeof(CurveSegment);
@ -691,17 +667,21 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
uint8_t nodeCount = ptr[0]; uint8_t nodeCount = ptr[0];
Serial.print("Node count: "); Serial.print("Node count: ");
Serial.println(nodeCount); Serial.println(nodeCount);
sendMessage(String("Node count: ") + String(nodeCount)); // sendMessage(String("Node count: ") + String(nodeCount));
sendMessage(String("ptr offset: ") + String(ptr - payload)); // sendMessage(String("ptr offset: ") + String(ptr - payload));
uint16_t remainingLength = length - (ptr - payload); uint16_t remainingLength = length - (ptr - payload);
if (remainingLength > 0) { if (remainingLength > 0) {
loadNodeGraph(ptr, remainingLength, animation.nodeGraph); loadNodeGraph(ptr, remainingLength, animation.nodeGraph);
animation.nodeGraph.bindAnimationContext(&animation);
} else { } else {
Serial.println("No node graph data found"); Serial.println("No node graph data found");
} }
sendMessage(printNodeGraph(animation.nodeGraph)); sendMessage(printNodeGraph(animation.nodeGraph));
currentAnimation = &animation;
currentAnimation->setActive(true); // ✅ mark it as running
// 🔹 Save using received filename // 🔹 Save using received filename
animation.saveToFile(("/" + String(filename)).c_str()); animation.saveToFile(("/" + String(filename)).c_str());
@ -849,6 +829,8 @@ bool flip = false;
unsigned long lastSend = 0; unsigned long lastSend = 0;
void loop() { void loop() {
runNodeAnimation();
// for (int i = 0; i < 1023; i++) { // for (int i = 0; i < 1023; i++) {
// servos[0]->sendWritePos(10, i); // servos[0]->sendWritePos(10, i);
// Serial.println(servos[0]->waitOnData1Byte(10)); // Serial.println(servos[0]->waitOnData1Byte(10));
@ -920,6 +902,64 @@ void loop() {
} }
} }
void runNodeAnimation() {
static uint32_t lastTickTime = 0;
static uint32_t currentTick = 0;
const uint16_t FRAME_INTERVAL_MS = 1000 / 48;
if (!currentAnimation) return; // ✅ Prevent crash
if (!currentAnimation->isActive()) return;
uint32_t now = millis();
if (now - lastTickTime >= FRAME_INTERVAL_MS) {
lastTickTime = now;
currentAnimation->nodeGraph.tick(currentTick, *currentAnimation);
// if (currentTick == 100){
// message += String(anim.getMotorPosition(10, currentTick));
// message += "\n";
// }
auto outputs = currentAnimation->nodeGraph.getServoOutputs();
std::vector<uint8_t> motorIDs;
std::vector<uint16_t> positions;
String message = String(currentTick) + String("\n");
for (const auto& [motorID, value] : outputs) {
motorIDs.push_back(motorID);
positions.push_back(map(value, 0, 4095, 0, 1023));
message += "Motor ";
message += String(motorID);
message += " → ";
message += String(value);
message += "\n";
}
uint8_t motorCount = motorIDs.size(); // ✅ number of motors
if (currentTick % 20 == 0) {
sendMessage(message);
}
// ✅ Send to servo controller
servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount);
currentTick++;
if (currentTick > currentAnimation->getFrameCount()) {
currentAnimation->setActive(false);
currentTick = 0; // optional: reset for next run
}
}
}
void SendMotorPositions() { void SendMotorPositions() {
uint16_t p0 = servos[0]->getPosition(10); uint16_t p0 = servos[0]->getPosition(10);
@ -1100,29 +1140,29 @@ void playAnimationOLD(Animation& anim) {
void playLayeredAnimation(Animation& base, Animation& overlay) { // void playLayeredAnimation(Animation& base, Animation& overlay) {
uint16_t basePositions[NUM_CHANNELS]; // uint16_t basePositions[NUM_CHANNELS];
uint16_t overlayPositions[NUM_CHANNELS]; // uint16_t overlayPositions[NUM_CHANNELS];
uint16_t finalPositions[NUM_CHANNELS]; // uint16_t finalPositions[NUM_CHANNELS];
const uint16_t frameCount = base.getFrameCount(); // const uint16_t frameCount = base.getFrameCount();
const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; // const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
uint32_t nextFrameTime = millis(); // uint32_t nextFrameTime = millis();
for (uint16_t frame = 0; frame < frameCount; frame++) { // for (uint16_t frame = 0; frame < frameCount; frame++) {
while (millis() < nextFrameTime) delay(1); // while (millis() < nextFrameTime) delay(1);
base.getFramePositions(frame, basePositions); // base.getFramePositions(frame, basePositions);
overlay.getFramePositions(frame, overlayPositions); // overlay.getFramePositions(frame, overlayPositions);
for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { // for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2; // finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
} // }
servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS); // servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS);
nextFrameTime += frameDelay; // nextFrameTime += frameDelay;
} // }
} // }
void SCSPingAll() { void SCSPingAll() {

69
animation.cpp
View File

@ -9,29 +9,30 @@ Animation::Animation() {
memset(header.reserved, 0, sizeof(header.reserved)); memset(header.reserved, 0, sizeof(header.reserved));
} }
void Animation::setActive(bool state) {
active = state;
}
void Animation::addCurveSegment(const CurveSegment& segment) { void Animation::addCurveSegment(const CurveSegment& segment) {
curves[segment.motorID].push_back(segment); curves[segment.motorID].push_back(segment);
} }
void Animation::clearCurves(uint8_t motorID) { void Animation::clearCurves(uint8_t motorID) {
curves.erase(motorID); // Completely remove the entry curves.erase(motorID); // Completely remove the entry
} }
void Animation::clearAllCurves() { void Animation::clearAllCurves() {
curves.clear(); // Wipe the entire map curves.clear(); // Wipe the entire map
} }
uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) { uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
if (motorID >= NUM_CHANNELS) return 0;
for (const auto& seg : curves[motorID]) { for (const auto& seg : curves[motorID]) {
if (timeCS >= seg.startTime && timeCS <= seg.endTime) { if (timeCS >= seg.startTime && timeCS <= seg.endTime) {
// Convert uint16_t to float in range -1 to 1 // Convert uint16_t to float in range -1 to 1
auto toFloat = [](int16_t v) {
return (float(v) / 65535.0f) * 2.0f - 1.0f;
};
// Define control points // Define control points
float x0 = seg.startTime; float x0 = seg.startTime;
@ -39,10 +40,11 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
float x2 = seg.endHandleX; float x2 = seg.endHandleX;
float x3 = seg.endTime; float x3 = seg.endTime;
float y0 = toFloat(seg.startPointY); float y0 = seg.startPointY;
float y1 = toFloat(seg.startHandleY); float y1 = seg.startHandleY;
float y2 = toFloat(seg.endHandleY); float y2 = seg.endHandleY;
float y3 = toFloat(seg.endPointY); float y3 = seg.endPointY;
// Solve for t such that Bézier x(t) ≈ timeCS // Solve for t such that Bézier x(t) ≈ timeCS
auto bezierX = [&](float t) { auto bezierX = [&](float t) {
@ -67,7 +69,8 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
float value = u * u * u * y0 + 3 * u * u * t * y1 + 3 * u * t * t * y2 + t * t * t * y3; float value = u * u * u * y0 + 3 * u * u * t * y1 + 3 * u * t * t * y2 + t * t * t * y3;
// Remap to PWM range // Remap to PWM range
return constrain((value + 1.0f) * 2047.5f, 0, 4095); return constrain(value, 0, 4095);
} }
} }
@ -169,40 +172,38 @@ bool Animation::loadFromFile(const char* filename) {
} }
void Animation::printCurves() { String Animation::printCurves() {
Serial.println("PRINTING CURVES"); String output = "PRINTING CURVES\n";
for (const auto& [motorID, segments] : curves) { for (const auto& [motorID, segments] : curves) {
Serial.print("Motor "); output += "Motor ";
Serial.print(motorID); output += String(motorID);
Serial.print(": "); output += ": ";
Serial.println(segments.size()); output += String(segments.size());
output += "\n";
for (const auto& seg : segments) { for (const auto& seg : segments) {
Serial.print(" Segment: "); output += " Segment: ";
Serial.print("startTime="); output += "startTime=" + String(seg.startTime);
Serial.print(seg.startTime); output += ", endTime=" + String(seg.endTime);
Serial.print(", endTime="); output += ", startPointY=" + String(seg.startPointY);
Serial.print(seg.endTime); output += ", startHandleX=" + String(seg.startHandleX);
Serial.print(", startPointY="); output += ", startHandleY=" + String(seg.startHandleY);
Serial.print(seg.startPointY); output += ", endHandleX=" + String(seg.endHandleX);
Serial.print(", startHandleX="); output += ", endHandleY=" + String(seg.endHandleY);
Serial.print(seg.startHandleX); output += ", endPointY=" + String(seg.endPointY);
Serial.print(", startHandleY="); output += "\n";
Serial.print(seg.startHandleY);
Serial.print(", endHandleX=");
Serial.print(seg.endHandleX);
Serial.print(", endHandleY=");
Serial.print(seg.endHandleY);
Serial.print(", endPointY=");
Serial.println(seg.endPointY);
} }
} }
return output;
} }
void Animation::createBasicSCurve() { void Animation::createBasicSCurve() {
clearAllCurves(); clearAllCurves();

9
animation.h
View File

@ -7,6 +7,7 @@
#include "nodegraph.h" #include "nodegraph.h"
#define NUM_CHANNELS 5 #define NUM_CHANNELS 5
#define FRAMES_PER_SECOND 50 #define FRAMES_PER_SECOND 50
#define MAX_DURATION_SECONDS 10 #define MAX_DURATION_SECONDS 10
@ -39,15 +40,17 @@ struct __attribute__((packed)) CurveSegment {
class Animation { class Animation {
public: public:
Animation(); Animation();
bool isActive() const { return active; }
void setActive(bool state);
void setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value); void setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value);
uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const; uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const;
bool getFramePositions(uint16_t frameIndex, uint16_t* outPositions);
void addCurveSegment(const CurveSegment& segment); void addCurveSegment(const CurveSegment& segment);
void clearCurves(uint8_t motorID); void clearCurves(uint8_t motorID);
void clearAllCurves(); void clearAllCurves();
void printCurves(); String printCurves();
uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS); uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS);
void clear(); void clear();
@ -66,6 +69,6 @@ public:
private: private:
//uint16_t data[MAX_FRAMES][NUM_CHANNELS]; //uint16_t data[MAX_FRAMES][NUM_CHANNELS];
std::unordered_map<uint8_t, std::vector<CurveSegment>> curves; std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;
bool active = false;
}; };

384
nodegraph.cpp
View File

@ -1,97 +1,305 @@
#include "nodegraph.h" #include "nodegraph.h"
#include "animation.h"
#include <cstring> #include <cstring>
// CurveNode evaluation // CurveNode evaluation
void CurveNode::evaluate(uint32_t tick) { void CurveNode::evaluate(uint32_t tick) {
//outputValue = getCurveValue(curveID, tick); if (!animation) {
outputValue = 2048;
return;
}
outputValue = animation->getMotorPosition(curveID, tick);
} }
Node* NodeGraph::findNodeByID(uint8_t id) const {
for (Node* node : nodes) {
if (node->id == id) {
return node;
}
}
return nullptr; // not found
}
// ServoNode evaluation // ServoNode evaluation
void ServoNode::evaluate(uint32_t tick) { void ServoNode::evaluate(uint32_t tick) {
//setMotorPWM(motorID, inputValue); outputValue = inputValue;
} }
extern uint16_t getSineWaveValue(uint32_t tick);
// extern int faceDetectX();
// extern int faceDetectY();
void VariableNode::evaluate(uint32_t currentTick) {
switch (source) {
case VAR_SINE:
outputValue = getSineWaveValue(currentTick);
break;
case VAR_FACE_X:
//outputValue = faceDetectX();
break;
case VAR_FACE_Y:
//outputValue = faceDetectY();
break;
case VAR_ANALOG:
outputValue = analogRead(7); // or whichever pin
break;
}
}
void MathNode::evaluate(uint32_t tick) {
float input = static_cast<float>(inputValue);
float result = 0;
switch (op) {
case OP_MULTIPLY: result = input * value; break;
case OP_DIVIDE: result = value != 0 ? input / value : 0; break;
case OP_ADD: result = input + value; break;
case OP_SUBTRACT: result = input - value; break;
}
outputValue = static_cast<uint16_t>(result);
}
void MapNode::evaluate(uint32_t tick) {
int32_t input = inputValue;
if (inMax == inMin) {
outputValue = outMin;
return;
}
int32_t result = (input - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
outputValue = static_cast<uint16_t>(result);
}
// NodeGraph tick // NodeGraph tick
void NodeGraph::tick(uint32_t currentTick) { void NodeGraph::tick(uint32_t currentTick, const Animation& animation) {
// First pass: evaluate all nodes // Step 1: Evaluate each node and propagate outputs immediately
for (Node* node : nodes) { for (Node* node : nodes) {
node->evaluate(currentTick); node->evaluate(currentTick);
}
// Optional: if you want to simulate wiring between nodes, for (const NodeConnection& conn : connections) {
// you could add a second pass here to propagate values if (conn.fromID == node->id) {
Node* to = findNodeByID(conn.toID);
if (to) {
to->inputValue = node->outputValue; // ✅ generic propagation
}
}
}
}
} }
std::vector<std::pair<uint8_t, uint16_t>> NodeGraph::getServoOutputs() const {
std::vector<std::pair<uint8_t, uint16_t>> outputs;
for (Node* node : nodes) {
if (node->type == TYPE_SERVONODE) {
const ServoNode* servo = static_cast<const ServoNode*>(node);
outputs.emplace_back(servo->motorID, servo->outputValue);
}
}
return outputs;
}
std::vector<Node*> NodeGraph::getSortedNodes() {
std::unordered_map<uint8_t, std::vector<uint8_t>> adj;
std::unordered_map<uint8_t, int> inDegree;
std::unordered_map<uint8_t, Node*> idMap;
for (Node* node : nodes) {
idMap[node->id] = node;
inDegree[node->id] = 0;
}
for (const NodeConnection& conn : connections) {
adj[conn.fromID].push_back(conn.toID);
inDegree[conn.toID]++;
}
std::vector<Node*> sorted;
std::queue<uint8_t> q;
for (const auto& [id, deg] : inDegree) {
if (deg == 0) q.push(id);
}
while (!q.empty()) {
uint8_t id = q.front();
q.pop();
sorted.push_back(idMap[id]);
for (uint8_t neighbor : adj[id]) {
if (--inDegree[neighbor] == 0) {
q.push(neighbor);
}
}
}
return sorted;
}
// This function links nodes to their outside inputs
void NodeGraph::bindAnimationContext(Animation* animation) {
for (Node* node : nodes) {
if (node->type == TYPE_CURVENODE) {
CurveNode* curveNode = static_cast<CurveNode*>(node);
curveNode->animation = animation; // ✅ link from outside
}
// Add other node types here as needed
}
}
void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) { void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
size_t offset = 0; size_t offset = 0;
// Read node count // Read node count
uint16_t nodeCount = packet[offset]; uint16_t nodeCount = packet[offset];
offset += 1; offset += 1;
// Parse nodes // Parse nodes
for (uint16_t i = 0; i < nodeCount; ++i) { for (uint16_t i = 0; i < nodeCount; ++i) {
if (offset + 6 > length) break; // safety check if (offset + 6 > length) break; // safety check
uint8_t type = packet[offset++]; uint8_t type = packet[offset++];
uint8_t id = packet[offset++]; uint8_t id = packet[offset++];
uint16_t x = packet[offset] | (packet[offset + 1] << 8); offset += 2; uint16_t x = packet[offset] | (packet[offset + 1] << 8);
uint16_t y = packet[offset] | (packet[offset + 1] << 8); offset += 2; offset += 2;
uint16_t y = packet[offset] | (packet[offset + 1] << 8);
offset += 2;
Node* node = nullptr; Node* node = nullptr;
switch (type) { switch (type) {
case TYPE_CURVENODE: { // CurveNode case TYPE_CURVENODE:
if (offset + 1 > length) break; { // CurveNode
uint8_t curveID = packet[offset++]; if (offset + 1 > length) break;
auto* curve = new CurveNode(); uint8_t curveID = packet[offset++];
curve->id = id; auto* curve = new CurveNode();
curve->type = type; curve->id = id;
curve->x = x; curve->type = type;
curve->y = y; curve->x = x;
curve->curveID = curveID; curve->y = y;
node = curve; curve->curveID = curveID;
break; node = curve;
} break;
case TYPE_SERVONODE: { // ServoNode }
if (offset + 1 > length) break; case TYPE_SERVONODE:
uint8_t motorID = packet[offset++]; { // ServoNode
auto* servo = new ServoNode(); if (offset + 1 > length) break;
servo->id = id; uint8_t motorID = packet[offset++];
servo->type = type; auto* servo = new ServoNode();
servo->x = x; servo->id = id;
servo->y = y; servo->type = type;
servo->motorID = motorID; servo->x = x;
node = servo; servo->y = y;
break; servo->motorID = motorID;
} node = servo;
case TYPE_NOISENODE: { // NoiseNode break;
if (offset + 17 > length) break; }
offset += 17; // skip for now case TYPE_VARIABLENODE:
break; { // ServoNode
} if (offset + 1 > length) break;
default: uint8_t source = packet[offset++];
break; auto* newNode = new VariableNode();
newNode->id = id;
newNode->type = type;
newNode->x = x;
newNode->y = y;
newNode->source = static_cast<VariableSource>(source);
node = newNode;
break;
}
case TYPE_MATHNODE:
{
if (offset + 5 > length) break; // 1 byte op + 4 bytes float
uint8_t rawOp = packet[offset++];
float val;
memcpy(&val, &packet[offset], sizeof(float));
offset += sizeof(float);
auto* math = new MathNode();
math->id = id;
math->type = type;
math->x = x;
math->y = y;
math->op = static_cast<MathOperator>(rawOp);
math->value = val;
node = math;
break;
}
case TYPE_MAPNODE:
{
if (offset + 16 > length) break; // 4 x float32 = 16 bytes
float inMin, inMax, outMin, outMax;
memcpy(&inMin, &packet[offset], sizeof(float));
offset += 4;
memcpy(&inMax, &packet[offset], sizeof(float));
offset += 4;
memcpy(&outMin, &packet[offset], sizeof(float));
offset += 4;
memcpy(&outMax, &packet[offset], sizeof(float));
offset += 4;
auto* mapNode = new MapNode();
mapNode->id = id;
mapNode->type = type;
mapNode->x = x;
mapNode->y = y;
mapNode->inMin = inMin;
mapNode->inMax = inMax;
mapNode->outMin = outMin;
mapNode->outMax = outMax;
node = mapNode;
break;
} }
if (node) {
graph.nodes.push_back(node);
case TYPE_NOISENODE:
{ // NoiseNode
if (offset + 17 > length) break;
offset += 17; // skip for now
break;
} }
default:
break;
} }
// Parse connections if (node) {
if (offset + 2 > length) return;
uint16_t connectionCount = packet[offset];
offset += 1;
for (uint16_t i = 0; i < connectionCount; ++i) { graph.nodes.push_back(node);
if (offset + 2 > length) break;
uint8_t fromID = packet[offset++];
uint8_t toID = packet[offset++];
graph.connections.push_back({fromID, toID});
} }
// Sort node list topologically for execution.
graph.nodes = graph.getSortedNodes();
}
// Parse connections
graph.connections.clear();
if (offset + 2 > length) return;
uint16_t connectionCount = packet[offset];
offset += 1;
for (uint16_t i = 0; i < connectionCount; ++i) {
if (offset + 2 > length) break;
uint8_t fromID = packet[offset++];
uint8_t toID = packet[offset++];
graph.connections.push_back({ fromID, toID });
}
} }
String printNodeGraph(const NodeGraph& graph) { String printNodeGraph(const NodeGraph& graph) {
@ -104,25 +312,33 @@ String printNodeGraph(const NodeGraph& graph) {
output += " | Pos (" + String(node->x) + ", " + String(node->y) + ")"; output += " | Pos (" + String(node->x) + ", " + String(node->y) + ")";
switch (node->type) { switch (node->type) {
case TYPE_CURVENODE: { // CurveNode case TYPE_CURVENODE:
const CurveNode* curve = static_cast<const CurveNode*>(node); { // CurveNode
output += " | CurveID " + String(curve->curveID); const CurveNode* curve = static_cast<const CurveNode*>(node);
break; output += " | CurveID " + String(curve->curveID);
} break;
case TYPE_SERVONODE: { // ServoNode }
const ServoNode* servo = static_cast<const ServoNode*>(node); case TYPE_SERVONODE:
output += " | MotorID " + String(servo->motorID); { // ServoNode
break; const ServoNode* servo = static_cast<const ServoNode*>(node);
} output += " | MotorID " + String(servo->motorID);
// case 2: { // NoiseNode break;
// const NoiseNode* noise = static_cast<const NoiseNode*>(node); }
// output += " | Noise a=" + String(noise->a, 2); case TYPE_VARIABLENODE:
// output += " b=" + String(noise->b, 2); { // VariableNode
// output += " c=" + String(noise->c, 2); const VariableNode* servo = static_cast<const VariableNode*>(node);
// output += " d=" + String(noise->d, 2); output += " | source: " + String(servo->source);
// output += " seed=" + String(noise->seed); break;
// break; }
// } // case 2: { // NoiseNode
// const NoiseNode* noise = static_cast<const NoiseNode*>(node);
// output += " | Noise a=" + String(noise->a, 2);
// output += " b=" + String(noise->b, 2);
// output += " c=" + String(noise->c, 2);
// output += " d=" + String(noise->d, 2);
// output += " seed=" + String(noise->seed);
// break;
// }
} }
output += "\n"; output += "\n";

57
nodegraph.h
View File

@ -1,13 +1,33 @@
#pragma once #pragma once
#include <vector> #include <vector>
#include <cstdint> #include <cstdint>
#include <queue>
#include <unordered_map>
#include <Arduino.h> #include <Arduino.h>
class Animation;
#define TYPE_NODE 0x01 #define TYPE_NODE 0x01
#define TYPE_SERVONODE 0x02 #define TYPE_SERVONODE 0x02
#define TYPE_CURVENODE 0x03 #define TYPE_CURVENODE 0x03
#define TYPE_NOISENODE 0x04 #define TYPE_NOISENODE 0x04
#define TYPE_VARIABLENODE 0x05
#define TYPE_MATHNODE 0x06
#define TYPE_MAPNODE 0x07
enum VariableSource {
VAR_FACE_X,
VAR_FACE_Y,
VAR_SINE,
VAR_ANALOG
};
enum MathOperator {
OP_MULTIPLY,
OP_DIVIDE,
OP_ADD,
OP_SUBTRACT
};
// Base Node class // Base Node class
@ -18,12 +38,14 @@ struct Node {
uint16_t y; uint16_t y;
virtual void evaluate(uint32_t tick) = 0; virtual void evaluate(uint32_t tick) = 0;
virtual ~Node() {} virtual ~Node() {}
uint16_t inputValue;
uint16_t outputValue = 0;
}; };
// CurveNode: evaluates a curve at a given tick // CurveNode: evaluates a curve at a given tick
struct CurveNode : public Node { struct CurveNode : public Node {
uint8_t curveID; uint8_t curveID;
uint16_t outputValue; Animation* animation = nullptr; // ✅ no need to include animation.h
void evaluate(uint32_t tick) override; void evaluate(uint32_t tick) override;
}; };
@ -31,11 +53,34 @@ struct CurveNode : public Node {
// ServoNode: sends a value to a motor // ServoNode: sends a value to a motor
struct ServoNode : public Node { struct ServoNode : public Node {
uint8_t motorID; uint8_t motorID;
uint16_t inputValue;
void evaluate(uint32_t tick) override; void evaluate(uint32_t tick) override;
}; };
struct VariableNode : public Node {
VariableSource source = VAR_SINE; // default
void evaluate(uint32_t tick) override;
void setSource(VariableSource src) { source = src; }
};
struct MathNode : public Node {
MathOperator op = OP_MULTIPLY;
float value = 1.0f;
void evaluate(uint32_t tick) override;
};
struct MapNode : public Node {
float inMin = 0;
float inMax = 1023;
float outMin = 0;
float outMax = 255;
void evaluate(uint32_t tick) override;
};
// NodeGraph container // NodeGraph container
struct NodeConnection { struct NodeConnection {
uint8_t fromID; uint8_t fromID;
@ -46,8 +91,14 @@ class NodeGraph {
public: public:
std::vector<Node*> nodes; std::vector<Node*> nodes;
std::vector<NodeConnection> connections; std::vector<NodeConnection> connections;
Node* findNodeByID(uint8_t id) const;
void tick(uint32_t currentTick, const Animation& animation);
std::vector<std::pair<uint8_t, uint16_t>> getServoOutputs() const;
std::vector<Node*> getSortedNodes();
void bindAnimationContext(Animation* animation);
void tick(uint32_t currentTick);
}; };