HansonServo/HansonServo.ino

#include <base64.h>
#include "feetech.h"
#include "animation.h"

#define DEVICE_NAME "Little Sophia"
#define FIRMWARE_VERSION "0.0.1"

#define HEADER1 0xAA
#define HEADER2 0x55
#define MAX_PAYLOAD_SIZE 6000       // 10 KB
uint8_t payload[MAX_PAYLOAD_SIZE];  // Global or static

#define CMD_ID_REQUEST 0x01
#define CMD_FILE_LIST 0x02
#define CMD_LOAD_FILE 0x03
#define CMD_DELETE_FILE 0x04
#define CMD_SAVE_FILE 0x05
#define CMD_MESSAGE 0x06
#define CMD_SET_POSITION 0x07


// ESP32 S2 PINOUT
#define RX_PIN 17  // DI
#define TX_PIN 18  // RO
#define DE_PIN 33  // Driver Enable
#define RE_PIN 3   // Receiver Enable

Animation anim;
Animation sweep;
Animation stagger;

Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);

uint16_t flipBytes(uint16_t value) {
  return (value >> 8) | (value << 8);
}


uint8_t ids[NUM_CHANNELS] = { 10, 11, 12, 13, 14 };
uint16_t pos1[] = { 0, 0, 0, 0, 0 };
uint16_t pos2[] = { 1023, 1023, 1023, 1023, 1023 };

void setup() {
  Serial.begin(1000000);
  for (int i = 0; i < 5; i++) {
    Serial.println(i);
    delay(500);
  }


  pos2[3] = flipBytes(pos2[3]);

  servos.begin();

  if (!FFat.begin()) {
    Serial.println("FFat mount failed");
    return;
  }

  // ClearFiles();
  // // PrintFileList();
  // sweep.clear();
  // sweep.createSampleSweep(4);
  // sweep.saveToFile("/sweep.anim");
  // stagger.clear();
  // stagger.createStaggeredSweep(4);
  // stagger.saveToFile("/stagger.anim");
  // //delay(9999);
  // sweep.printKeyframes();


  // anim.clear();
  // if (!sweep.loadFromFile("/sweep.anim")) {
  //   Serial.println("Failed to load animation");
  //   return;
  // } else {
  //   Serial.println("Loaded sweep anim");
  // }

  // if (!stagger.loadFromFile("/stagger.anim")) {
  //   Serial.println("Failed to load animation");
  //   return;
  // } else {
  //   Serial.println("Loaded stagger anim");
  // }


  // Serial.println(anim.getFrame(0, 0));  // Should show saved value

  //SetID(11, 14);
}

void SetID(uint8_t oldID, uint8_t newID) {
  Serial.println("Setting Lock to 0");
  Serial.println(servos.setLock(oldID, 0));
  delay(1000);
  Serial.print("Changing ID ");
  Serial.print(oldID);
  Serial.print(" to ");
  Serial.println(newID);
  Serial.println(servos.setID(oldID, newID));
  delay(1000);
  Serial.println("Setting Lock to 1");
  Serial.println(servos.setLock(newID, 1));
  delay(1000);
}

void HandleSerialRequests() {
  if (Serial.available() >= 6) {  // 2 headers + 1 command + 2 length + 1 checksum minimum
    if (Serial.read() == HEADER1 && Serial.read() == HEADER2) {
      uint8_t command = Serial.read();
      uint16_t length = (Serial.read() << 8) | Serial.read();

      if (length > MAX_PAYLOAD_SIZE) {
        Serial.println("Payload too large");
        while (Serial.available()) Serial.read();  // flush junk
        return;
      }

      unsigned long start = millis();
      while (Serial.available() < length + 1) {
        if (millis() - start > 1000) {  // 1 second timeout
          Serial.println("Serial timeout");
          return;
        }
      }


      uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF);

      for (uint16_t i = 0; i < length; i++) {
        payload[i] = Serial.read();
        checksum ^= payload[i];
      }

      uint8_t receivedChecksum = Serial.read();

      if (checksum == receivedChecksum) {
        handleCommand(command, payload, length);
      } else {
        Serial.println("Checksum mismatch");
      }
    }
  }
}

void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
  switch (command) {
    case CMD_ID_REQUEST:  // CMD_ID_REQUEST
      handleIdRequest();
      break;

    case CMD_FILE_LIST:  //
      handleFileList();
      break;

    case CMD_LOAD_FILE:  //
      handleLoadFile(payload, length);
      break;

    case CMD_DELETE_FILE:  //
      handleDeleteFile(payload, length);
      break;

    case CMD_SAVE_FILE:
      handleSaveFile(payload, length);
      break;

    case CMD_SET_POSITION:
      handleSetPosition(payload, length);
      break;

    default:
      Serial.print("Unknown command: ");
      Serial.println(command, HEX);
      break;
  }
}

void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
  uint16_t length = payload.length();

  uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF);
  for (int i = 0; i < length; i++) {
    checksum ^= payload[i];
  }

  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(command);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);
  Serial.write((const uint8_t*)payload.c_str(), length);
  Serial.write(checksum);
}

void handleIdRequest() {
  String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
  uint16_t length = payload.length();

  uint8_t checksum = CMD_ID_REQUEST ^ (length >> 8) ^ (length & 0xFF);
  for (int i = 0; i < length; i++) {
    checksum ^= payload[i];
  }

  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);
  Serial.write(checksum);
}

void handleFileList() {
  File root = FFat.open("/");
  if (!root || !root.isDirectory()) {
    sendFileListResponse("");  // empty payload
    return;
  }

  String payload = "";
  File file = root.openNextFile();

  while (file) {
    if (!file.isDirectory()) {
      payload += String(file.name()) + "\n";
    }
    file = root.openNextFile();
  }

  sendFileListResponse(payload);
}

void sendFileListResponse(const String& payloadStr) {
  uint16_t length = payloadStr.length();
  if (length > MAX_PAYLOAD_SIZE) {
    Serial.println("File list too large");
    return;
  }

  uint8_t checksum = CMD_FILE_LIST ^ (length >> 8) ^ (length & 0xFF);
  for (uint16_t i = 0; i < length; i++) {
    checksum ^= payloadStr[i];
  }

  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(CMD_FILE_LIST);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);
  Serial.write((const uint8_t*)payloadStr.c_str(), length);
  Serial.write(checksum);
}

void handleLoadFile(const uint8_t* payload, uint16_t length) {
  if (length == 0 || length >= 128) {
    Serial.println("Invalid filename");
    return;
  }

  char filename[128];
  memcpy(filename, payload, length);
  filename[length] = '\0';

  File file = FFat.open(filename, "r");
  if (!file || !file.available()) {
    Serial.println("File not found or empty");
    return;
  }

  size_t fileSize = file.size();
  if (fileSize > 65535) {
    Serial.println("File too large for single transfer");
    file.close();
    return;
  }

  uint8_t* buffer = (uint8_t*)malloc(fileSize);
  if (!buffer) {
    Serial.println("Memory allocation failed");
    file.close();
    return;
  }

  size_t bytesRead = file.read(buffer, fileSize);
  file.close();

  if (bytesRead != fileSize) {
    Serial.println("File read error");
    free(buffer);
    return;
  }

  // 🔹 Compute checksum
  uint8_t checksum = CMD_LOAD_FILE ^ (fileSize >> 8) ^ (fileSize & 0xFF);
  for (size_t i = 0; i < fileSize; i++) {
    checksum ^= buffer[i];
  }

  // 🔹 Send packet
  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(CMD_LOAD_FILE);
  Serial.write((fileSize >> 8) & 0xFF);
  Serial.write(fileSize & 0xFF);
  Serial.write(buffer, fileSize);
  Serial.write(checksum);

  free(buffer);
  Serial.println("File sent in one go");
}

void handleDeleteFile(const uint8_t* payload, uint16_t length) {
  sendMessage("Deleting FILE");
  if (length < 1) {
    Serial.println("Payload too short for filename length");
    sendMessage("Payload too short for filename length");
    return;
  }

  // 🔹 Parse filename
  uint16_t filenameLength = payload[0] | (payload[1] << 8);
  if (length < 2 + filenameLength) {
    Serial.println("Payload too short for filename");
    sendMessage("Payload too short for filename");
    return;
  }


  char filename[filenameLength + 1];


  memcpy(filename, payload + 2, filenameLength);
  filename[filenameLength] = '\0';

  deleteFile(FFat, ("/" + String(filename)).c_str());


  sendMessage(("File Deleted: " + String(filename)).c_str(), CMD_DELETE_FILE);
}

void handleSaveFile(const uint8_t* payload, uint16_t length) {
  bool valid = parseAnimationPayload(payload, length, anim);

  if (valid) {
    //Serial.println("Animation parsed successfully!");
    //anim.printKeyframes();
  } else {
    //Serial.println("Failed to parse animation.");
    length = 1;         // Override length to 1 for fallback response
    payload = nullptr;  // We'll send a single 0x00 instead
  }

  if (length > MAX_PAYLOAD_SIZE) {
    Serial.println("File list too large");
    return;
  }

  // Calculate checksum
  uint8_t checksum = CMD_SAVE_FILE ^ (length >> 8) ^ (length & 0xFF);
  if (valid) {
    for (uint16_t i = 0; i < length; i++) {
      checksum ^= payload[i];
    }
  } else {
    checksum ^= 0x00;
  }

  // Send response
  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(CMD_SAVE_FILE);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);

  if (valid) {
    for (uint16_t i = 0; i < length; i++) {
      Serial.write(payload[i]);
    }
  } else {
    Serial.write(0x00);
  }

  Serial.write(checksum);
}



bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& animation) {
  sendMessage("SAVING FILE");
  if (length < 1) {
    Serial.println("Payload too short for filename length");
    return false;
  }

  // 🔹 Parse filename
  uint16_t filenameLength = payload[0] | (payload[1] << 8);
  if (length < 2 + filenameLength + 18) {
    Serial.println("Payload too short for filename and header");
    return false;
  }

  char filename[filenameLength + 1];


  memcpy(filename, payload + 2, filenameLength);
  filename[filenameLength] = '\0';


  const uint8_t* ptr = payload + 2 + filenameLength;

  String msg = "SAVING FILE: ";
  msg += filename;
  sendMessage(msg);
  // 🔹 Parse header
  memcpy(animation.header.magic, ptr, 4);
  if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
    Serial.println("Invalid magic header");

    sendMessage("invalid magic header");
    return false;
  }

  animation.header.frameCount = (ptr[4] << 8) | ptr[5];
  animation.header.version = ptr[6];
  animation.header.frameRate = ptr[7];
  memcpy(animation.header.reserved, ptr + 8, 8);

  uint16_t keyframeCount = ptr[16] | (ptr[17] << 8);
  ptr += 18;

  if (length < (ptr - payload) + keyframeCount * 5) {
    Serial.println("Payload too short for keyframes");
    sendMessage("Payload too short");
    return false;
  }

  // 🔹 Parse keyframes
  animation.clear();
  for (uint16_t i = 0; i < keyframeCount; i++) {
    uint8_t motorId = ptr[0];
    uint16_t frame = ptr[1] | (ptr[2] << 8);
    uint16_t position = ptr[3] | (ptr[4] << 8);
    animation.addKeyframe(motorId, frame, position);
    ptr += 5;
  }

  // 🔹 Save using received filename
  animation.saveToFile(("/" + String(filename)).c_str());

  sendMessage("SAVED");
  return true;
}


void handleSetPosition(const uint8_t* payload, uint16_t length) {
  if (length % 3 != 0) {
    Serial.println("Invalid sync packet length");
    return;
  }

  uint8_t count = 0;
  for (int i = 0; i < length; i += 3) {
    uint8_t motorId = payload[i];
    uint16_t position = (payload[i + 2] << 8) | payload[i + 1];
    pos1[count] = position;
    count++;
    // Apply position to motorId

    //servos.sendWritePos(ids[motorId], position);
  }
  servos.syncWritePos(ids, pos1, NUM_CHANNELS);
  // String msg = "Set positions: ";
  // for (int i = 0; i < count; i++) {
  //   msg += "ID " + String(ids[i]) + " → " + String(pos1[i]) + "; ";
  // }
  // sendMessage(msg.c_str(), CMD_SET_POSITION);
}




bool flip = false;
unsigned long lastSend = 0;
void loop() {
  HandleSerialRequests();
  // put your main code here, to run repeatedly:
  //PingAll();

  // for (int i = 0; i < 1023; i++) {
  //   for (int z = 0; z < 5; z++) {
  //     pos1[z] = i;
  //   }
  //   servos.syncWritePos(ids, pos1, 5);
  //   delay(50);
  // }

  // playAnimation(sweep);
  // playAnimation(stagger);
  // playLayeredAnimation(sweep, stagger);


  // servos.syncWritePos(ids, pos1, 5);
  // delay(1000);

  // servos.syncWritePos(ids, pos2, 5);
  // delay(1000);

  if (millis() - lastSend > 2000) {

    //sendMessageFromESP32(String(millis()));
    //handleIdRequest();
    //PrintFileList();
    lastSend = millis();
  }
}



void PrintFileList() {
  File root = FFat.open("/");
  if (!root || !root.isDirectory()) {
    Serial.println("Failed to open FFat root directory");
    return;
  }

  Serial.println("Files in FFat:");

  File file = root.openNextFile();
  while (file) {
    Serial.print("  ");
    Serial.print(file.name());
    Serial.print("  |  Size: ");
    Serial.println(file.size());
    file = root.openNextFile();
  }

  Serial.println("End of file list.");
}

void ClearFiles() {
  File root = FFat.open("/");
  if (!root || !root.isDirectory()) {
    Serial.println("Failed to open FFat root directory");
    return;
  }

  Serial.println("Files in FFat:");

  File file = root.openNextFile();
  while (file) {
    String filename = "/" + String(file.name());  // Add leading slash
    Serial.print("  Deleting: ");
    Serial.println(filename);
    file.close();                        // Close before deleting
    deleteFile(FFat, filename.c_str());  // Use corrected path
    file = root.openNextFile();
  }

  Serial.println("FFat cleanup complete.");
}



void deleteFile(fs::FS& fs, const char* path) {
  Serial.printf("Deleting file: %s\r\n", path);
  if (fs.remove(path)) {
    Serial.println("- file deleted");
  } else {
    Serial.println("- delete failed");
  }
}

void playAnimation(Animation& anim) {
  uint16_t positions[NUM_CHANNELS];
  const uint16_t frameCount = anim.getFrameCount();
  const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;  // 20 ms

  uint32_t nextFrameTime = millis();

  for (uint16_t frame = 0; frame < frameCount; frame++) {
    // Wait until it's time for the next frame
    while (millis() < nextFrameTime) {
      // Optional: yield or do background tasks here
      delay(1);
    }

    // Send frame to servos
    if (anim.getFramePositions(frame, positions)) {
      servos.syncWritePos(ids, positions, NUM_CHANNELS);
    }

    // Schedule next frame
    nextFrameTime += frameDelay;
  }
}

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();

  for (uint16_t frame = 0; frame < frameCount; frame++) {
    while (millis() < nextFrameTime) delay(1);

    base.getFramePositions(frame, basePositions);
    overlay.getFramePositions(frame, overlayPositions);

    for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
      finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
    }

    servos.syncWritePos(ids, finalPositions, NUM_CHANNELS);
    nextFrameTime += frameDelay;
  }
}



void PingAll() {
  std::vector<uint8_t> successfulAddresses;
  servos.pingAll(successfulAddresses);

  // Now successfulAddresses contains all successful pings
  Serial.println("Successful Addresses:");
  for (uint8_t address : successfulAddresses) {
    Serial.print(address);
    Serial.print(" ");
  }
  Serial.println();
}