variable, math, and map() nodes are implemented
parent
29758d8abf
commit
eaedb1ac3f
204
HansonServo.ino
204
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<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() {
|
||||
|
||||
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() {
|
||||
|
|
|
|||
|
|
@ -9,6 +9,11 @@ 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);
|
||||
}
|
||||
|
|
@ -24,14 +29,10 @@ void Animation::clearAllCurves() {
|
|||
|
||||
|
||||
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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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<uint8_t, std::vector<CurveSegment>> curves;
|
||||
|
||||
bool active = false;
|
||||
};
|
||||
|
||||
|
|
|
|||
250
nodegraph.cpp
250
nodegraph.cpp
|
|
@ -1,27 +1,165 @@
|
|||
#include "nodegraph.h"
|
||||
#include "animation.h"
|
||||
#include <cstring>
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// NodeGraph tick
|
||||
void NodeGraph::tick(uint32_t currentTick) {
|
||||
// First pass: evaluate all nodes
|
||||
for (Node* node : nodes) {
|
||||
node->evaluate(currentTick);
|
||||
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;
|
||||
}
|
||||
|
||||
// Optional: if you want to simulate wiring between nodes,
|
||||
// you could add a second pass here to propagate values
|
||||
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
|
||||
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);
|
||||
|
||||
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<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) {
|
||||
size_t offset = 0;
|
||||
|
||||
|
|
@ -35,13 +173,16 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
|
|||
|
||||
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;
|
||||
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;
|
||||
|
||||
switch (type) {
|
||||
case TYPE_CURVENODE: { // CurveNode
|
||||
case TYPE_CURVENODE:
|
||||
{ // CurveNode
|
||||
if (offset + 1 > length) break;
|
||||
uint8_t curveID = packet[offset++];
|
||||
auto* curve = new CurveNode();
|
||||
|
|
@ -53,7 +194,8 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
|
|||
node = curve;
|
||||
break;
|
||||
}
|
||||
case TYPE_SERVONODE: { // ServoNode
|
||||
case TYPE_SERVONODE:
|
||||
{ // ServoNode
|
||||
if (offset + 1 > length) break;
|
||||
uint8_t motorID = packet[offset++];
|
||||
auto* servo = new ServoNode();
|
||||
|
|
@ -65,7 +207,68 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
|
|||
node = servo;
|
||||
break;
|
||||
}
|
||||
case TYPE_NOISENODE: { // NoiseNode
|
||||
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<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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
case TYPE_NOISENODE:
|
||||
{ // NoiseNode
|
||||
if (offset + 17 > length) break;
|
||||
offset += 17; // skip for now
|
||||
break;
|
||||
|
|
@ -78,9 +281,14 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
|
|||
|
||||
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;
|
||||
|
|
@ -90,7 +298,7 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
|
|||
uint8_t fromID = packet[offset++];
|
||||
uint8_t toID = packet[offset++];
|
||||
|
||||
graph.connections.push_back({fromID, toID});
|
||||
graph.connections.push_back({ fromID, toID });
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -104,16 +312,24 @@ String printNodeGraph(const NodeGraph& graph) {
|
|||
output += " | Pos (" + String(node->x) + ", " + String(node->y) + ")";
|
||||
|
||||
switch (node->type) {
|
||||
case TYPE_CURVENODE: { // CurveNode
|
||||
case TYPE_CURVENODE:
|
||||
{ // CurveNode
|
||||
const CurveNode* curve = static_cast<const CurveNode*>(node);
|
||||
output += " | CurveID " + String(curve->curveID);
|
||||
break;
|
||||
}
|
||||
case TYPE_SERVONODE: { // ServoNode
|
||||
case TYPE_SERVONODE:
|
||||
{ // ServoNode
|
||||
const ServoNode* servo = static_cast<const ServoNode*>(node);
|
||||
output += " | MotorID " + String(servo->motorID);
|
||||
break;
|
||||
}
|
||||
case TYPE_VARIABLENODE:
|
||||
{ // VariableNode
|
||||
const VariableNode* servo = static_cast<const VariableNode*>(node);
|
||||
output += " | source: " + String(servo->source);
|
||||
break;
|
||||
}
|
||||
// case 2: { // NoiseNode
|
||||
// const NoiseNode* noise = static_cast<const NoiseNode*>(node);
|
||||
// output += " | Noise a=" + String(noise->a, 2);
|
||||
|
|
|
|||
57
nodegraph.h
57
nodegraph.h
|
|
@ -1,13 +1,33 @@
|
|||
#pragma once
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
#include <queue>
|
||||
#include <unordered_map>
|
||||
#include <Arduino.h>
|
||||
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<Node*> nodes;
|
||||
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);
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue