From eaedb1ac3f72ee27a9ccb19708986c872d1fb9cd Mon Sep 17 00:00:00 2001 From: Jake Date: Sat, 18 Oct 2025 23:21:24 +0800 Subject: [PATCH] variable, math, and map() nodes are implemented --- HansonServo.ino | 204 ++++++++++++++----------- animation.cpp | 69 ++++----- animation.h | 9 +- nodegraph.cpp | 384 +++++++++++++++++++++++++++++++++++++----------- nodegraph.h | 57 ++++++- 5 files changed, 517 insertions(+), 206 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index 8e21dd6..cf97f43 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -38,8 +38,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static #define RE_PIN 8 // Receiver Enable Animation anim; -Animation sweep; -Animation stagger; +Animation* currentAnimation; bool streamPositions = false; unsigned long lastStreamPositions = 0; @@ -59,45 +58,15 @@ uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 }; uint16_t pos1STS[] = { 0, 0 }; uint16_t pos2STS[] = { 1023, 1023 }; -const uint8_t testPayload[] = { - // πŸ”Ή Filename block (length = 17) - 17, 0, - '/', 'p', 'o', 'i', 'n', 't', 'c', 'u', 'r', 'v', 'e', '2', '.', 'a', 'n', 'i', 'm', +#define WAVE_PERIOD_CS 400 // 4 seconds = 400 centiseconds +#define WAVE_MAX 4095 - // πŸ”Ή Header: "ANIM", frameCount=800, version=1, frameRate=50, reserved[8] - 'A', 'N', 'I', 'M', - 32, 3, // frameCount = 800 (little-endian) - 1, // version - 50, // frameRate - 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); +uint16_t getSineWaveValue(unsigned long centiseconds) { + float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS; + float sine = sin(theta); // ranges from -1 to +1 + float scaled = (sine + 1.0) * (WAVE_MAX / 2.0); // scale to 0–4095 + return (uint16_t)round(scaled); +} void setup() { @@ -108,6 +77,9 @@ void setup() { 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]->setFeetechMode(Feetech::MODE_SCS); 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++) { CurveSegment seg; memcpy(&seg, ptr, sizeof(CurveSegment)); - Serial.print("Segment "); - Serial.print(i); - Serial.print(": motorID="); - Serial.print(seg.motorID); - Serial.print(", startTime="); - Serial.print(seg.startTime); - Serial.print(", endTime="); - Serial.print(seg.endTime); - Serial.print(", startPointY="); - Serial.print(seg.startPointY); - Serial.print(", startHandleX="); - Serial.print(seg.startHandleX); - Serial.print(", startHandleY="); - 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); + // Serial.print("Segment "); + // Serial.print(i); + // Serial.print(": motorID="); + // Serial.print(seg.motorID); + // Serial.print(", startTime="); + // Serial.print(seg.startTime); + // Serial.print(", endTime="); + // Serial.print(seg.endTime); + // Serial.print(", startPointY="); + // Serial.print(seg.startPointY); + // Serial.print(", startHandleX="); + // Serial.print(seg.startHandleX); + // Serial.print(", startHandleY="); + // 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); + animation.addCurveSegment(seg); 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 //ptr += curveCount * sizeof(CurveSegment); @@ -691,17 +667,21 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a uint8_t nodeCount = ptr[0]; Serial.print("Node count: "); Serial.println(nodeCount); - sendMessage(String("Node count: ") + String(nodeCount)); - sendMessage(String("ptr offset: ") + String(ptr - payload)); + // sendMessage(String("Node count: ") + String(nodeCount)); + // sendMessage(String("ptr offset: ") + String(ptr - payload)); uint16_t remainingLength = length - (ptr - payload); if (remainingLength > 0) { loadNodeGraph(ptr, remainingLength, animation.nodeGraph); + animation.nodeGraph.bindAnimationContext(&animation); } else { 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 animation.saveToFile(("/" + String(filename)).c_str()); @@ -849,6 +829,8 @@ bool flip = false; unsigned long lastSend = 0; void loop() { + + runNodeAnimation(); // for (int i = 0; i < 1023; i++) { // servos[0]->sendWritePos(10, i); // 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 motorIDs; + std::vector 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() { uint16_t p0 = servos[0]->getPosition(10); @@ -1100,29 +1140,29 @@ void playAnimationOLD(Animation& anim) { -void playLayeredAnimation(Animation& base, Animation& overlay) { - uint16_t basePositions[NUM_CHANNELS]; - uint16_t overlayPositions[NUM_CHANNELS]; - uint16_t finalPositions[NUM_CHANNELS]; +// void playLayeredAnimation(Animation& base, Animation& overlay) { +// uint16_t basePositions[NUM_CHANNELS]; +// uint16_t overlayPositions[NUM_CHANNELS]; +// uint16_t finalPositions[NUM_CHANNELS]; - const uint16_t frameCount = base.getFrameCount(); - const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; - uint32_t nextFrameTime = millis(); +// const uint16_t frameCount = base.getFrameCount(); +// const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; +// uint32_t nextFrameTime = millis(); - for (uint16_t frame = 0; frame < frameCount; frame++) { - while (millis() < nextFrameTime) delay(1); +// for (uint16_t frame = 0; frame < frameCount; frame++) { +// while (millis() < nextFrameTime) delay(1); - base.getFramePositions(frame, basePositions); - overlay.getFramePositions(frame, overlayPositions); +// base.getFramePositions(frame, basePositions); +// overlay.getFramePositions(frame, overlayPositions); - for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { - finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2; - } +// for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { +// finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2; +// } - servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS); - nextFrameTime += frameDelay; - } -} +// servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS); +// nextFrameTime += frameDelay; +// } +// } void SCSPingAll() { diff --git a/animation.cpp b/animation.cpp index a353112..e689295 100644 --- a/animation.cpp +++ b/animation.cpp @@ -9,29 +9,30 @@ Animation::Animation() { memset(header.reserved, 0, sizeof(header.reserved)); } +void Animation::setActive(bool state) { + active = state; +} + + void Animation::addCurveSegment(const CurveSegment& segment) { curves[segment.motorID].push_back(segment); } void Animation::clearCurves(uint8_t motorID) { - curves.erase(motorID); // Completely remove the entry + curves.erase(motorID); // Completely remove the entry } 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) { - if (motorID >= NUM_CHANNELS) return 0; for (const auto& seg : curves[motorID]) { if (timeCS >= seg.startTime && timeCS <= seg.endTime) { // 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 float x0 = seg.startTime; @@ -39,10 +40,11 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) { float x2 = seg.endHandleX; float x3 = seg.endTime; - float y0 = toFloat(seg.startPointY); - float y1 = toFloat(seg.startHandleY); - float y2 = toFloat(seg.endHandleY); - float y3 = toFloat(seg.endPointY); + float y0 = seg.startPointY; + float y1 = seg.startHandleY; + float y2 = seg.endHandleY; + float y3 = seg.endPointY; + // Solve for t such that BΓ©zier x(t) β‰ˆ timeCS 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; // 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() { - Serial.println("PRINTING CURVES"); +String Animation::printCurves() { + String output = "PRINTING CURVES\n"; + for (const auto& [motorID, segments] : curves) { - Serial.print("Motor "); - Serial.print(motorID); - Serial.print(": "); - Serial.println(segments.size()); + output += "Motor "; + output += String(motorID); + output += ": "; + output += String(segments.size()); + output += "\n"; for (const auto& seg : segments) { - Serial.print(" Segment: "); - Serial.print("startTime="); - Serial.print(seg.startTime); - Serial.print(", endTime="); - Serial.print(seg.endTime); - Serial.print(", startPointY="); - Serial.print(seg.startPointY); - Serial.print(", startHandleX="); - Serial.print(seg.startHandleX); - Serial.print(", startHandleY="); - 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); + output += " Segment: "; + output += "startTime=" + String(seg.startTime); + output += ", endTime=" + String(seg.endTime); + output += ", startPointY=" + String(seg.startPointY); + output += ", startHandleX=" + String(seg.startHandleX); + output += ", startHandleY=" + String(seg.startHandleY); + output += ", endHandleX=" + String(seg.endHandleX); + output += ", endHandleY=" + String(seg.endHandleY); + output += ", endPointY=" + String(seg.endPointY); + output += "\n"; } } + + return output; } + void Animation::createBasicSCurve() { clearAllCurves(); diff --git a/animation.h b/animation.h index 0fd2054..000a56d 100644 --- a/animation.h +++ b/animation.h @@ -7,6 +7,7 @@ #include "nodegraph.h" + #define NUM_CHANNELS 5 #define FRAMES_PER_SECOND 50 #define MAX_DURATION_SECONDS 10 @@ -39,15 +40,17 @@ struct __attribute__((packed)) CurveSegment { class Animation { public: Animation(); + bool isActive() const { return active; } + void setActive(bool state); + void setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value); uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const; - bool getFramePositions(uint16_t frameIndex, uint16_t* outPositions); void addCurveSegment(const CurveSegment& segment); void clearCurves(uint8_t motorID); void clearAllCurves(); - void printCurves(); + String printCurves(); uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS); void clear(); @@ -66,6 +69,6 @@ public: private: //uint16_t data[MAX_FRAMES][NUM_CHANNELS]; std::unordered_map> curves; - + bool active = false; }; diff --git a/nodegraph.cpp b/nodegraph.cpp index f80fe1c..c21e471 100644 --- a/nodegraph.cpp +++ b/nodegraph.cpp @@ -1,97 +1,305 @@ #include "nodegraph.h" +#include "animation.h" #include // CurveNode evaluation 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 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(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(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(result); +} + + // NodeGraph tick -void NodeGraph::tick(uint32_t currentTick) { - // First pass: evaluate all nodes - for (Node* node : nodes) { - node->evaluate(currentTick); - } +void NodeGraph::tick(uint32_t currentTick, const Animation& animation) { + // Step 1: Evaluate each node and propagate outputs immediately + for (Node* node : nodes) { + node->evaluate(currentTick); - // Optional: if you want to simulate wiring between nodes, - // you could add a second pass here to propagate values + for (const NodeConnection& conn : connections) { + if (conn.fromID == node->id) { + Node* to = findNodeByID(conn.toID); + if (to) { + to->inputValue = node->outputValue; // βœ… generic propagation + } + } + } + } } + + +std::vector> NodeGraph::getServoOutputs() const { + std::vector> outputs; + + for (Node* node : nodes) { + if (node->type == TYPE_SERVONODE) { + const ServoNode* servo = static_cast(node); + outputs.emplace_back(servo->motorID, servo->outputValue); + } + } + + return outputs; +} + + +std::vector NodeGraph::getSortedNodes() { + std::unordered_map> adj; + std::unordered_map inDegree; + std::unordered_map 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 sorted; + std::queue 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(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) { - size_t offset = 0; + size_t offset = 0; - // Read node count - uint16_t nodeCount = packet[offset]; - offset += 1; + // Read node count + uint16_t nodeCount = packet[offset]; + offset += 1; - // Parse nodes - for (uint16_t i = 0; i < nodeCount; ++i) { - if (offset + 6 > length) break; // safety check + // Parse nodes + for (uint16_t i = 0; i < nodeCount; ++i) { + if (offset + 6 > length) break; // safety check - uint8_t type = packet[offset++]; - uint8_t id = packet[offset++]; - uint16_t x = packet[offset] | (packet[offset + 1] << 8); offset += 2; - uint16_t y = packet[offset] | (packet[offset + 1] << 8); offset += 2; + uint8_t type = packet[offset++]; + uint8_t id = packet[offset++]; + uint16_t x = packet[offset] | (packet[offset + 1] << 8); + offset += 2; + uint16_t y = packet[offset] | (packet[offset + 1] << 8); + offset += 2; - Node* node = nullptr; + Node* node = nullptr; - switch (type) { - case TYPE_CURVENODE: { // CurveNode - if (offset + 1 > length) break; - uint8_t curveID = packet[offset++]; - auto* curve = new CurveNode(); - curve->id = id; - curve->type = type; - curve->x = x; - curve->y = y; - curve->curveID = curveID; - node = curve; - break; - } - case TYPE_SERVONODE: { // ServoNode - if (offset + 1 > length) break; - uint8_t motorID = packet[offset++]; - auto* servo = new ServoNode(); - servo->id = id; - servo->type = type; - servo->x = x; - servo->y = y; - servo->motorID = motorID; - node = servo; - break; - } - case TYPE_NOISENODE: { // NoiseNode - if (offset + 17 > length) break; - offset += 17; // skip for now - break; - } - default: - break; + switch (type) { + case TYPE_CURVENODE: + { // CurveNode + if (offset + 1 > length) break; + uint8_t curveID = packet[offset++]; + auto* curve = new CurveNode(); + curve->id = id; + curve->type = type; + curve->x = x; + curve->y = y; + curve->curveID = curveID; + node = curve; + break; + } + case TYPE_SERVONODE: + { // ServoNode + if (offset + 1 > length) break; + uint8_t motorID = packet[offset++]; + auto* servo = new ServoNode(); + servo->id = id; + servo->type = type; + servo->x = x; + servo->y = y; + servo->motorID = motorID; + node = servo; + break; + } + case TYPE_VARIABLENODE: + { // ServoNode + if (offset + 1 > length) break; + uint8_t source = packet[offset++]; + auto* newNode = new VariableNode(); + newNode->id = id; + newNode->type = type; + newNode->x = x; + newNode->y = y; + newNode->source = static_cast(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(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 (offset + 2 > length) return; - uint16_t connectionCount = packet[offset]; - offset += 1; + if (node) { - 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}); + graph.nodes.push_back(node); } + + // 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) { @@ -104,25 +312,33 @@ String printNodeGraph(const NodeGraph& graph) { output += " | Pos (" + String(node->x) + ", " + String(node->y) + ")"; switch (node->type) { - case TYPE_CURVENODE: { // CurveNode - const CurveNode* curve = static_cast(node); - output += " | CurveID " + String(curve->curveID); - break; - } - case TYPE_SERVONODE: { // ServoNode - const ServoNode* servo = static_cast(node); - output += " | MotorID " + String(servo->motorID); - break; - } - // case 2: { // NoiseNode - // const NoiseNode* noise = static_cast(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; - // } + case TYPE_CURVENODE: + { // CurveNode + const CurveNode* curve = static_cast(node); + output += " | CurveID " + String(curve->curveID); + break; + } + case TYPE_SERVONODE: + { // ServoNode + const ServoNode* servo = static_cast(node); + output += " | MotorID " + String(servo->motorID); + break; + } + case TYPE_VARIABLENODE: + { // VariableNode + const VariableNode* servo = static_cast(node); + output += " | source: " + String(servo->source); + break; + } + // case 2: { // NoiseNode + // const NoiseNode* noise = static_cast(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"; diff --git a/nodegraph.h b/nodegraph.h index 4f137c3..e71b174 100644 --- a/nodegraph.h +++ b/nodegraph.h @@ -1,13 +1,33 @@ #pragma once #include #include +#include +#include #include +class Animation; #define TYPE_NODE 0x01 #define TYPE_SERVONODE 0x02 #define TYPE_CURVENODE 0x03 #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 @@ -18,12 +38,14 @@ struct Node { uint16_t y; virtual void evaluate(uint32_t tick) = 0; virtual ~Node() {} + uint16_t inputValue; + uint16_t outputValue = 0; }; // CurveNode: evaluates a curve at a given tick struct CurveNode : public Node { uint8_t curveID; - uint16_t outputValue; + Animation* animation = nullptr; // βœ… no need to include animation.h void evaluate(uint32_t tick) override; }; @@ -31,11 +53,34 @@ struct CurveNode : public Node { // ServoNode: sends a value to a motor struct ServoNode : public Node { uint8_t motorID; - uint16_t inputValue; 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 struct NodeConnection { uint8_t fromID; @@ -46,8 +91,14 @@ class NodeGraph { public: std::vector nodes; std::vector connections; +Node* findNodeByID(uint8_t id) const; + + void tick(uint32_t currentTick, const Animation& animation); + std::vector> getServoOutputs() const; + std::vector getSortedNodes(); + void bindAnimationContext(Animation* animation); + - void tick(uint32_t currentTick); };