NoiseNode implemented (octave and persistence hard coded), added servo feedback option to variable
parent
eaedb1ac3f
commit
6b991fa982
137
HansonServo.ino
137
HansonServo.ino
|
|
@ -3,6 +3,9 @@
|
|||
#include "feetech.h"
|
||||
#include "animation.h"
|
||||
#include "nodegraph.h"
|
||||
#include "RobotConfig.h"
|
||||
|
||||
RobotConfig config;
|
||||
|
||||
#define DEVICE_NAME "Little Sophia"
|
||||
#define FIRMWARE_VERSION "0.0.1"
|
||||
|
|
@ -37,6 +40,13 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
|
|||
#define DE_PIN 7 // Driver Enable
|
||||
#define RE_PIN 8 // Receiver Enable
|
||||
|
||||
#define PLAY_IDLE 0x00
|
||||
#define PLAY_ONCE 0x01
|
||||
#define PLAY_LOOP 0x02
|
||||
#define PLAY_REPEAT 0x03
|
||||
uint8_t playMode = PLAY_IDLE;
|
||||
uint8_t repeatsRemaining = 0;
|
||||
|
||||
Animation anim;
|
||||
Animation* currentAnimation;
|
||||
|
||||
|
|
@ -77,6 +87,29 @@ void setup() {
|
|||
delay(500);
|
||||
}
|
||||
|
||||
config.deviceName = "Little Elephant";
|
||||
config.firmwareVersion.major = 0;
|
||||
config.firmwareVersion.minor = 2;
|
||||
|
||||
// Add a few motors
|
||||
config.motors.push_back({ "Neck Twist", 10, 2048 });
|
||||
config.motors.push_back({ "Left Shoulder", 11, 2048 });
|
||||
config.motors.push_back({ "Right Shoulder", 12, 2048 });
|
||||
config.motors.push_back({ "Pelvis Tilt", 13, 2048 });
|
||||
config.motors.push_back({ "Neck Tilt", 14, 2048 });
|
||||
|
||||
// Print serialized config
|
||||
Serial.println(config.serializeJSON());
|
||||
std::vector<uint8_t> bytes = config.serializeToBytes();
|
||||
String output = "";
|
||||
|
||||
for (size_t i = 0; i < bytes.size(); ++i) {
|
||||
if (bytes[i] < 16) output += "0"; // pad single-digit hex
|
||||
output += String(bytes[i], HEX);
|
||||
if (i < bytes.size() - 1) output += " ";
|
||||
}
|
||||
Serial.println(output);
|
||||
|
||||
pinMode(6, INPUT);
|
||||
pinMode(7, INPUT);
|
||||
|
||||
|
|
@ -349,23 +382,32 @@ void handleDataWrite(const uint8_t* payload, uint16_t length) {
|
|||
}
|
||||
|
||||
void handleIdRequest() {
|
||||
String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
|
||||
uint16_t length = payload.length();
|
||||
std::vector<uint8_t> payload = config.serializeToBytes();
|
||||
uint16_t length = payload.size();
|
||||
|
||||
// Compute checksum: XOR of CMD_ID_REQUEST, length bytes, and all payload bytes
|
||||
uint8_t checksum = CMD_ID_REQUEST ^ (length >> 8) ^ (length & 0xFF);
|
||||
for (int i = 0; i < length; i++) {
|
||||
checksum ^= payload[i];
|
||||
for (uint8_t byte : payload) {
|
||||
checksum ^= byte;
|
||||
}
|
||||
|
||||
// Send header
|
||||
Serial.write(HEADER1);
|
||||
Serial.write(HEADER2);
|
||||
Serial.write(CMD_ID_REQUEST);
|
||||
Serial.write((length >> 8) & 0xFF);
|
||||
Serial.write(length & 0xFF);
|
||||
Serial.write((const uint8_t*)payload.c_str(), length);
|
||||
|
||||
// Send payload
|
||||
for (uint8_t byte : payload) {
|
||||
Serial.write(byte);
|
||||
}
|
||||
|
||||
// Send checksum
|
||||
Serial.write(checksum);
|
||||
}
|
||||
|
||||
|
||||
void handleFileList() {
|
||||
File root = FFat.open("/");
|
||||
if (!root || !root.isDirectory()) {
|
||||
|
|
@ -514,14 +556,20 @@ void handlePlayAnimation(const uint8_t* payload, uint16_t length) {
|
|||
|
||||
char filename[filenameLength + 1];
|
||||
|
||||
|
||||
memcpy(filename, payload + 2, filenameLength);
|
||||
filename[filenameLength] = '\0';
|
||||
|
||||
// 🔹 Extract playback mode and repeat count
|
||||
playMode = payload[2 + filenameLength];
|
||||
repeatsRemaining = payload[3 + filenameLength];
|
||||
|
||||
//deleteFile(FFat, ("/" + String(filename)).c_str());
|
||||
anim.clear();
|
||||
anim.loadFromFile(("/" + String(filename)).c_str());
|
||||
playAnimation(anim);
|
||||
//playAnimation(anim);
|
||||
|
||||
currentAnimation = &anim;
|
||||
currentAnimation->setActive(true); // ✅ mark it as running
|
||||
|
||||
sendMessage("File Played", CMD_PLAY_FILE);
|
||||
}
|
||||
|
|
@ -680,8 +728,8 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
|
|||
}
|
||||
sendMessage(printNodeGraph(animation.nodeGraph));
|
||||
|
||||
currentAnimation = &animation;
|
||||
currentAnimation->setActive(true); // ✅ mark it as running
|
||||
// currentAnimation = &animation;
|
||||
// currentAnimation->setActive(true); // ✅ mark it as running
|
||||
|
||||
// 🔹 Save using received filename
|
||||
animation.saveToFile(("/" + String(filename)).c_str());
|
||||
|
|
@ -886,11 +934,12 @@ void loop() {
|
|||
// Serial.println(servos[1]->getGoalSpeed(13));
|
||||
// delay(10);
|
||||
// }
|
||||
if (millis() - lastSend > 2000) {
|
||||
|
||||
//sendMessageFromESP32(String(millis()));
|
||||
//handleIdRequest();
|
||||
//PrintFileList();
|
||||
if (millis() - lastSend > 50) {
|
||||
// Update config motor positions
|
||||
for (const Motor& motor : config.motors) {
|
||||
uint16_t position = servos[0]->getPosition(motor.motorID);
|
||||
config.setMotorPosition(motor.motorID, position);
|
||||
}
|
||||
lastSend = millis();
|
||||
}
|
||||
|
||||
|
|
@ -902,6 +951,8 @@ void loop() {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void runNodeAnimation() {
|
||||
static uint32_t lastTickTime = 0;
|
||||
static uint32_t currentTick = 0;
|
||||
|
|
@ -910,6 +961,8 @@ void runNodeAnimation() {
|
|||
if (!currentAnimation) return; // ✅ Prevent crash
|
||||
if (!currentAnimation->isActive()) return;
|
||||
|
||||
config.enableAllMotors();
|
||||
|
||||
uint32_t now = millis();
|
||||
if (now - lastTickTime >= FRAME_INTERVAL_MS) {
|
||||
lastTickTime = now;
|
||||
|
|
@ -928,9 +981,18 @@ void runNodeAnimation() {
|
|||
String message = String(currentTick) + String("\n");
|
||||
|
||||
for (const auto& [motorID, value] : outputs) {
|
||||
if (value != 65535) {
|
||||
motorIDs.push_back(motorID);
|
||||
positions.push_back(map(value, 0, 4095, 0, 1023));
|
||||
|
||||
positions.push_back(value);
|
||||
config.setMotorPosition(motorID, value);
|
||||
if (config.setMotorEnabled(motorID, true)) {
|
||||
servos[0]->enableTorque(motorID);
|
||||
}
|
||||
} else {
|
||||
if (config.setMotorEnabled(motorID, false)) {
|
||||
servos[0]->disableTorque(motorID);
|
||||
}
|
||||
}
|
||||
message += "Motor ";
|
||||
message += String(motorID);
|
||||
message += " → ";
|
||||
|
|
@ -948,12 +1010,24 @@ void runNodeAnimation() {
|
|||
servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount);
|
||||
|
||||
|
||||
|
||||
currentTick++;
|
||||
|
||||
if (currentTick > currentAnimation->getFrameCount()) {
|
||||
switch (playMode) {
|
||||
case PLAY_ONCE:
|
||||
currentAnimation->setActive(false);
|
||||
currentTick = 0; // optional: reset for next run
|
||||
break;
|
||||
case PLAY_LOOP:
|
||||
|
||||
break;
|
||||
case PLAY_REPEAT:
|
||||
repeatsRemaining--;
|
||||
if (repeatsRemaining == 0) {
|
||||
currentAnimation->setActive(false);
|
||||
}
|
||||
break;
|
||||
}
|
||||
currentTick = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -961,29 +1035,18 @@ void runNodeAnimation() {
|
|||
|
||||
|
||||
void SendMotorPositions() {
|
||||
std::vector<uint8_t> payload;
|
||||
|
||||
uint16_t p0 = servos[0]->getPosition(10);
|
||||
uint16_t p1 = servos[0]->getPosition(11);
|
||||
uint16_t p2 = servos[0]->getPosition(12);
|
||||
uint16_t p3 = servos[0]->getPosition(13);
|
||||
uint16_t p4 = servos[0]->getPosition(14);
|
||||
for (const Motor& motor : config.motors) {
|
||||
uint16_t position = motor.position;
|
||||
payload.push_back(position >> 8); // High byte
|
||||
payload.push_back(position & 0xFF); // Low byte
|
||||
}
|
||||
|
||||
uint8_t payload[10];
|
||||
|
||||
payload[0] = p0 >> 8;
|
||||
payload[1] = p0 & 0xFF;
|
||||
payload[2] = p1 >> 8;
|
||||
payload[3] = p1 & 0xFF;
|
||||
payload[4] = p2 >> 8;
|
||||
payload[5] = p2 & 0xFF;
|
||||
payload[6] = p3 >> 8;
|
||||
payload[7] = p3 & 0xFF;
|
||||
payload[8] = p4 >> 8;
|
||||
payload[9] = p4 & 0xFF;
|
||||
|
||||
sendMessage(payload, 10, POSITION_STREAM);
|
||||
sendMessage(payload.data(), payload.size(), POSITION_STREAM);
|
||||
}
|
||||
|
||||
|
||||
void PrintFileList() {
|
||||
File root = FFat.open("/");
|
||||
if (!root || !root.isDirectory()) {
|
||||
|
|
|
|||
|
|
@ -70,7 +70,6 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
|
|||
|
||||
// Remap to PWM range
|
||||
return constrain(value, 0, 4095);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -104,29 +103,28 @@ bool Animation::saveToFile(const char* filename) {
|
|||
File file = FFat.open(filename, FILE_WRITE);
|
||||
if (!file) return false;
|
||||
|
||||
// Write header
|
||||
file.write((uint8_t*)&header, sizeof(header));
|
||||
|
||||
// Count total curve segments
|
||||
uint16_t curveCount = 0;
|
||||
for (const auto& [motorID, segments] : curves) {
|
||||
curveCount += segments.size();
|
||||
}
|
||||
|
||||
// Write curve count
|
||||
file.write((uint8_t*)&curveCount, sizeof(curveCount));
|
||||
|
||||
// Write all curve segments
|
||||
for (const auto& [motorID, segments] : curves) {
|
||||
for (const CurveSegment& seg : segments) {
|
||||
file.write((uint8_t*)&seg, sizeof(CurveSegment));
|
||||
}
|
||||
}
|
||||
|
||||
// ✅ Write serialized node graph
|
||||
std::vector<uint8_t> graphData = nodeGraph.serialize();
|
||||
file.write(graphData.data(), graphData.size());
|
||||
|
||||
file.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool Animation::loadFromFile(const char* filename) {
|
||||
File file = FFat.open(filename, FILE_READ);
|
||||
if (!file) return false;
|
||||
|
|
@ -152,7 +150,6 @@ bool Animation::loadFromFile(const char* filename) {
|
|||
return false;
|
||||
}
|
||||
|
||||
// Clear existing curves
|
||||
clearAllCurves();
|
||||
|
||||
// Read curve segments
|
||||
|
|
@ -162,16 +159,31 @@ bool Animation::loadFromFile(const char* filename) {
|
|||
file.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
// Store segment directly — no motorID filtering
|
||||
curves[seg.motorID].push_back(seg);
|
||||
}
|
||||
|
||||
// ✅ Read remaining bytes into buffer
|
||||
size_t remaining = file.available();
|
||||
if (remaining > 0) {
|
||||
std::vector<uint8_t> buffer(remaining);
|
||||
if (file.read(buffer.data(), remaining) != remaining) {
|
||||
file.close();
|
||||
return false;
|
||||
}
|
||||
|
||||
// ✅ Load node graph from buffer
|
||||
nodeGraph.nodes.clear();
|
||||
nodeGraph.connections.clear();
|
||||
loadNodeGraph(buffer.data(), buffer.size(), nodeGraph);
|
||||
nodeGraph.bindAnimationContext(this);
|
||||
}
|
||||
|
||||
file.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
String Animation::printCurves() {
|
||||
String output = "PRINTING CURVES\n";
|
||||
|
||||
|
|
|
|||
15
feetech.cpp
15
feetech.cpp
|
|
@ -46,10 +46,19 @@ void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) {
|
|||
packet[index++] = DATA_LEN_PER_SERVO;
|
||||
|
||||
// Servo data
|
||||
uint16_t pos;
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
packet[index++] = ids[i];
|
||||
packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte
|
||||
packet[index++] = (uint8_t)(positions[i] & 0xFF); // Low byte
|
||||
|
||||
if (feetechMode == MODE_SCS) {
|
||||
pos = map(positions[i], 0, 4095, 0, 1023);
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
pos = positions[i];
|
||||
}
|
||||
|
||||
|
||||
packet[index++] = (uint8_t)((pos >> 8) & 0xFF); // High byte
|
||||
packet[index++] = (uint8_t)(pos & 0xFF); // Low byte
|
||||
packet[index++] = 0x00; // Speed high byte
|
||||
packet[index++] = 0x00; // Speed low byte
|
||||
}
|
||||
|
|
@ -336,7 +345,7 @@ uint16_t Feetech::getPosition(uint8_t id) {
|
|||
if (feetechMode == MODE_STS) {
|
||||
return waitOnData2Bytes(10);
|
||||
} else if (feetechMode == MODE_SCS) {
|
||||
return flipBytes(waitOnData2Bytes(10));
|
||||
return map(flipBytes(waitOnData2Bytes(10)), 0, 1023, 0, 4095);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
144
nodegraph.cpp
144
nodegraph.cpp
|
|
@ -1,7 +1,12 @@
|
|||
#include "nodegraph.h"
|
||||
#include "animation.h"
|
||||
#include "noise.h"
|
||||
#include "RobotConfig.h"
|
||||
#include <cstring>
|
||||
|
||||
extern RobotConfig config;
|
||||
|
||||
|
||||
// CurveNode evaluation
|
||||
void CurveNode::evaluate(uint32_t tick) {
|
||||
if (!animation) {
|
||||
|
|
@ -22,6 +27,13 @@ Node* NodeGraph::findNodeByID(uint8_t id) const {
|
|||
return nullptr; // not found
|
||||
}
|
||||
|
||||
void NoiseNode::evaluate(uint32_t tick) {
|
||||
float t = (static_cast<float>(tick) / 480.0f) * frequency * 10.0f;
|
||||
|
||||
float raw = perlin1D_octave(seed, t, 4, 0.5f);
|
||||
outputValue = (raw + 1.0f) * 2047.5f; // → [0, 4095]
|
||||
}
|
||||
|
||||
|
||||
// ServoNode evaluation
|
||||
void ServoNode::evaluate(uint32_t tick) {
|
||||
|
|
@ -46,6 +58,9 @@ void VariableNode::evaluate(uint32_t currentTick) {
|
|||
case VAR_ANALOG:
|
||||
outputValue = analogRead(7); // or whichever pin
|
||||
break;
|
||||
case VAR_SERVO_FEEDBACK:
|
||||
outputValue = config.getMotorPosition(arg0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -133,7 +148,13 @@ std::vector<Node*> NodeGraph::getSortedNodes() {
|
|||
while (!q.empty()) {
|
||||
uint8_t id = q.front();
|
||||
q.pop();
|
||||
sorted.push_back(idMap[id]);
|
||||
Node* node = idMap[id];
|
||||
|
||||
// ✅ Reset values here
|
||||
node->inputValue = 65535;
|
||||
node->outputValue = 65535;
|
||||
|
||||
sorted.push_back(node);
|
||||
|
||||
for (uint8_t neighbor : adj[id]) {
|
||||
if (--inDegree[neighbor] == 0) {
|
||||
|
|
@ -145,6 +166,7 @@ std::vector<Node*> NodeGraph::getSortedNodes() {
|
|||
return sorted;
|
||||
}
|
||||
|
||||
|
||||
// This function links nodes to their outside inputs
|
||||
void NodeGraph::bindAnimationContext(Animation* animation) {
|
||||
for (Node* node : nodes) {
|
||||
|
|
@ -211,12 +233,14 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
|
|||
{ // ServoNode
|
||||
if (offset + 1 > length) break;
|
||||
uint8_t source = packet[offset++];
|
||||
uint8_t arg0 = packet[offset++];
|
||||
auto* newNode = new VariableNode();
|
||||
newNode->id = id;
|
||||
newNode->type = type;
|
||||
newNode->x = x;
|
||||
newNode->y = y;
|
||||
newNode->source = static_cast<VariableSource>(source);
|
||||
newNode->arg0 = arg0;
|
||||
node = newNode;
|
||||
break;
|
||||
}
|
||||
|
|
@ -264,14 +288,27 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
|
|||
node = mapNode;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
case TYPE_NOISENODE:
|
||||
{ // NoiseNode
|
||||
if (offset + 17 > length) break;
|
||||
offset += 17; // skip for now
|
||||
break;
|
||||
{
|
||||
if (offset + 6 > length) break; // 4 bytes for float + 2 bytes for uint16_t
|
||||
|
||||
float frequency;
|
||||
uint16_t seed;
|
||||
|
||||
memcpy(&frequency, &packet[offset], sizeof(float));
|
||||
offset += 4;
|
||||
|
||||
memcpy(&seed, &packet[offset], sizeof(uint16_t));
|
||||
offset += 2;
|
||||
|
||||
auto* noiseNode = new NoiseNode();
|
||||
noiseNode->id = id;
|
||||
noiseNode->type = type;
|
||||
noiseNode->x = x;
|
||||
noiseNode->y = y;
|
||||
noiseNode->frequency = frequency;
|
||||
noiseNode->seed = seed;
|
||||
node = noiseNode;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
|
|
@ -328,17 +365,16 @@ String printNodeGraph(const NodeGraph& graph) {
|
|||
{ // VariableNode
|
||||
const VariableNode* servo = static_cast<const VariableNode*>(node);
|
||||
output += " | source: " + String(servo->source);
|
||||
output += " | arg0: " + String(servo->arg0);
|
||||
break;
|
||||
}
|
||||
case TYPE_NOISENODE:
|
||||
{ // NoiseNode
|
||||
const NoiseNode* noise = static_cast<const NoiseNode*>(node);
|
||||
output += " | Frequency " + String(noise->frequency, 4);
|
||||
output += " | Seed " + String(noise->seed);
|
||||
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";
|
||||
|
|
@ -351,3 +387,77 @@ String printNodeGraph(const NodeGraph& graph) {
|
|||
|
||||
return output;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> NodeGraph::serialize() {
|
||||
std::vector<uint8_t> data;
|
||||
|
||||
// Node count
|
||||
data.push_back(static_cast<uint8_t>(nodes.size()));
|
||||
|
||||
// Serialize nodes
|
||||
for (Node* node : nodes) {
|
||||
data.push_back(node->type);
|
||||
data.push_back(node->id);
|
||||
|
||||
uint16_t x = node->x;
|
||||
uint16_t y = node->y;
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&x), reinterpret_cast<uint8_t*>(&x) + sizeof(uint16_t));
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&y), reinterpret_cast<uint8_t*>(&y) + sizeof(uint16_t));
|
||||
|
||||
switch (node->type) {
|
||||
case TYPE_SERVONODE:
|
||||
{
|
||||
auto* n = static_cast<ServoNode*>(node);
|
||||
data.push_back(n->motorID);
|
||||
break;
|
||||
}
|
||||
case TYPE_CURVENODE:
|
||||
{
|
||||
auto* n = static_cast<CurveNode*>(node);
|
||||
data.push_back(n->curveID);
|
||||
break;
|
||||
}
|
||||
case TYPE_VARIABLENODE:
|
||||
{
|
||||
auto* n = static_cast<VariableNode*>(node);
|
||||
data.push_back(static_cast<uint8_t>(n->source));
|
||||
data.push_back(static_cast<uint8_t>(n->arg0));
|
||||
break;
|
||||
}
|
||||
case TYPE_MATHNODE:
|
||||
{
|
||||
auto* n = static_cast<MathNode*>(node);
|
||||
data.push_back(static_cast<uint8_t>(n->op));
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->value), reinterpret_cast<uint8_t*>(&n->value) + sizeof(float));
|
||||
break;
|
||||
}
|
||||
case TYPE_MAPNODE:
|
||||
{
|
||||
auto* n = static_cast<MapNode*>(node);
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->inMin), reinterpret_cast<uint8_t*>(&n->inMin) + sizeof(float));
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->inMax), reinterpret_cast<uint8_t*>(&n->inMax) + sizeof(float));
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->outMin), reinterpret_cast<uint8_t*>(&n->outMin) + sizeof(float));
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->outMax), reinterpret_cast<uint8_t*>(&n->outMax) + sizeof(float));
|
||||
break;
|
||||
}
|
||||
case TYPE_NOISENODE:
|
||||
{
|
||||
auto* n = static_cast<NoiseNode*>(node);
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->frequency), reinterpret_cast<uint8_t*>(&n->frequency) + sizeof(float));
|
||||
data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->seed), reinterpret_cast<uint8_t*>(&n->seed) + sizeof(uint16_t));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Connection count
|
||||
data.push_back(static_cast<uint8_t>(connections.size()));
|
||||
|
||||
// Serialize connections
|
||||
for (const NodeConnection& conn : connections) {
|
||||
data.push_back(conn.fromID);
|
||||
data.push_back(conn.toID);
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
|
|
|
|||
21
nodegraph.h
21
nodegraph.h
|
|
@ -18,7 +18,8 @@ enum VariableSource {
|
|||
VAR_FACE_X,
|
||||
VAR_FACE_Y,
|
||||
VAR_SINE,
|
||||
VAR_ANALOG
|
||||
VAR_ANALOG,
|
||||
VAR_SERVO_FEEDBACK
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -50,6 +51,14 @@ struct CurveNode : public Node {
|
|||
void evaluate(uint32_t tick) override;
|
||||
};
|
||||
|
||||
//
|
||||
struct NoiseNode : public Node {
|
||||
float frequency;
|
||||
uint16_t seed;
|
||||
|
||||
void evaluate(uint32_t tick) override;
|
||||
};
|
||||
|
||||
// ServoNode: sends a value to a motor
|
||||
struct ServoNode : public Node {
|
||||
uint8_t motorID;
|
||||
|
|
@ -59,9 +68,12 @@ struct ServoNode : public Node {
|
|||
|
||||
struct VariableNode : public Node {
|
||||
VariableSource source = VAR_SINE; // default
|
||||
uint8_t arg0;
|
||||
|
||||
void evaluate(uint32_t tick) override;
|
||||
void setSource(VariableSource src) { source = src; }
|
||||
void setSource(VariableSource src) {
|
||||
source = src;
|
||||
}
|
||||
};
|
||||
|
||||
struct MathNode : public Node {
|
||||
|
|
@ -91,14 +103,13 @@ class NodeGraph {
|
|||
public:
|
||||
std::vector<Node*> nodes;
|
||||
std::vector<NodeConnection> connections;
|
||||
Node* findNodeByID(uint8_t id) const;
|
||||
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);
|
||||
|
||||
|
||||
std::vector<uint8_t> serialize();
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,78 @@
|
|||
#include "noise.h"
|
||||
#include <math.h>
|
||||
#include <algorithm>
|
||||
|
||||
|
||||
uint8_t perm[512];
|
||||
static uint16_t currentSeed = 0xFFFF;
|
||||
|
||||
|
||||
inline static float fade(float t) {
|
||||
return t * t * t * (t * (t * 6 - 15) + 10);
|
||||
}
|
||||
|
||||
inline static float noiseLerp(float a, float b, float t) {
|
||||
return a + t * (b - a);
|
||||
}
|
||||
|
||||
inline static float grad(uint8_t hash, float x) {
|
||||
uint8_t h = hash & 15;
|
||||
float grad = 1.0f + (h & 7); // 1 to 8
|
||||
if (h & 8) grad = -grad;
|
||||
return (grad * x) / 4.0f; // match JS scaling
|
||||
}
|
||||
|
||||
static void generatePermutation(uint16_t seed, uint8_t* perm) {
|
||||
uint8_t p[256];
|
||||
uint32_t s = seed;
|
||||
|
||||
for (int i = 0; i < 256; i++) {
|
||||
s = (s * 1664525 + 1013904223) % 4294967296;
|
||||
p[i] = i;
|
||||
}
|
||||
|
||||
for (int i = 255; i > 0; i--) {
|
||||
uint32_t j = s % (i + 1);
|
||||
std::swap(p[i], p[j]);
|
||||
s = (s * 1664525 + 1013904223) % 4294967296;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 512; i++) {
|
||||
perm[i] = p[i & 255];
|
||||
}
|
||||
}
|
||||
|
||||
float perlin1D_octave(uint16_t seed, float x, int octaves = 4, float persistence = 0.5f) {
|
||||
|
||||
if (seed != currentSeed) {
|
||||
generatePermutation(seed, perm);
|
||||
currentSeed = seed;
|
||||
}
|
||||
|
||||
float total = 0.0f;
|
||||
float amplitude = 1.0f;
|
||||
float frequency = 1.0f;
|
||||
float maxValue = 0.0f;
|
||||
|
||||
for (int i = 0; i < octaves; i++) {
|
||||
total += perlin1D(seed, x * frequency) * amplitude;
|
||||
maxValue += amplitude;
|
||||
amplitude *= persistence;
|
||||
frequency *= 2.0f;
|
||||
}
|
||||
|
||||
return total / maxValue;
|
||||
}
|
||||
|
||||
|
||||
float perlin1D(uint16_t seed, float x) {
|
||||
|
||||
int xi = static_cast<int>(floor(x)) & 255;
|
||||
float xf = x - floor(x);
|
||||
float u = fade(xf);
|
||||
|
||||
uint8_t a = perm[xi];
|
||||
uint8_t b = perm[xi + 1];
|
||||
|
||||
return noiseLerp(grad(a, xf), grad(b, xf - 1.0f), u);
|
||||
}
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
#pragma once
|
||||
#include <stdint.h>
|
||||
|
||||
extern uint8_t perm[512];
|
||||
float perlin1D_octave(uint16_t seed, float x, int octaves, float persistence);
|
||||
float perlin1D(uint16_t seed, float x);
|
||||
|
|
@ -0,0 +1,114 @@
|
|||
#include "RobotConfig.h"
|
||||
|
||||
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
||||
for (const Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
return motor.position;
|
||||
}
|
||||
}
|
||||
// Return 0 if motor not found
|
||||
return 2047;
|
||||
}
|
||||
|
||||
bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
|
||||
for (Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
motor.position = newPosition;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
// Return false if motor not found
|
||||
return false;
|
||||
}
|
||||
|
||||
bool RobotConfig::setMotorEnabled(uint8_t motorID, bool enable) {
|
||||
for (Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
if (motor.isEnabled != enable) {
|
||||
motor.isEnabled = enable;
|
||||
return true; // ✅ Only return true if the value changed
|
||||
}
|
||||
return false; // ❌ No change
|
||||
}
|
||||
}
|
||||
return false; // ❌ Motor not found
|
||||
}
|
||||
|
||||
|
||||
bool RobotConfig::isMotorEnabled(uint8_t motorID) {
|
||||
for (Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
return motor.isEnabled;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void RobotConfig::enableAllMotors() {
|
||||
for (Motor& motor : motors) {
|
||||
motor.isEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
String RobotConfig::serializeJSON() const {
|
||||
String output = "{";
|
||||
output += "\"deviceName\":\"" + deviceName + "\",";
|
||||
output += "\"firmwareVersion\":{\"major\":" + String(firmwareVersion.major) + ",\"minor\":" + String(firmwareVersion.minor) + "},";
|
||||
|
||||
output += "\"motors\":[";
|
||||
|
||||
for (size_t i = 0; i < motors.size(); ++i) {
|
||||
const Motor& m = motors[i];
|
||||
output += "{";
|
||||
output += "\"motorID\":" + String(m.motorID) + ",";
|
||||
output += "\"name\":\"" + m.name + "\",";
|
||||
output += "\"position\":" + String(m.position);
|
||||
output += "}";
|
||||
|
||||
if (i < motors.size() - 1) {
|
||||
output += ",";
|
||||
}
|
||||
}
|
||||
|
||||
output += "]}";
|
||||
return output;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> RobotConfig::serializeToBytes() const {
|
||||
std::vector<uint8_t> buffer;
|
||||
|
||||
// Encode deviceName length + characters
|
||||
uint8_t nameLen = deviceName.length();
|
||||
buffer.push_back(nameLen);
|
||||
for (uint8_t i = 0; i < nameLen; ++i) {
|
||||
buffer.push_back(deviceName[i]);
|
||||
}
|
||||
|
||||
// Encode firmwareVersion (int32_t → 4 bytes)
|
||||
buffer.push_back(firmwareVersion.major);
|
||||
buffer.push_back(firmwareVersion.minor);
|
||||
|
||||
|
||||
// Encode number of motors
|
||||
uint8_t motorCount = motors.size();
|
||||
buffer.push_back(motorCount);
|
||||
|
||||
// Encode each motor
|
||||
for (const Motor& m : motors) {
|
||||
// motorID
|
||||
buffer.push_back(m.motorID);
|
||||
|
||||
// name length + characters
|
||||
uint8_t motorNameLen = m.name.length();
|
||||
buffer.push_back(motorNameLen);
|
||||
for (uint8_t i = 0; i < motorNameLen; ++i) {
|
||||
buffer.push_back(m.name[i]);
|
||||
}
|
||||
|
||||
// position (uint16_t → 2 bytes)
|
||||
buffer.push_back((m.position >> 8) & 0xFF);
|
||||
buffer.push_back(m.position & 0xFF);
|
||||
}
|
||||
|
||||
return buffer;
|
||||
}
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <vector>
|
||||
|
||||
|
||||
struct FirmwareVersion {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
};
|
||||
|
||||
struct Motor {
|
||||
String name; // Optional: name or ID of the motor
|
||||
uint8_t motorID;
|
||||
uint16_t position; // Current position in encoder ticks or degrees
|
||||
bool isEnabled = true;
|
||||
};
|
||||
|
||||
struct RobotConfig {
|
||||
String deviceName; // Name of the robot
|
||||
FirmwareVersion firmwareVersion;
|
||||
std::vector<Motor> motors; // Dynamic array of motors
|
||||
|
||||
|
||||
uint16_t getMotorPosition(uint8_t motorID) const;
|
||||
bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
|
||||
bool setMotorEnabled(uint8_t motorID,bool enable);
|
||||
bool isMotorEnabled(uint8_t motorID);
|
||||
void enableAllMotors();
|
||||
String serializeJSON() const;
|
||||
std::vector<uint8_t> serializeToBytes() const;
|
||||
};
|
||||
Loading…
Reference in New Issue