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

sync option implemented, motors move with dials and timeline shifts

master
Jake 2025-09-29 23:05:13 +08:00
parent 051fe5c8c2
commit d7ff7b3155
1 changed files with 67 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
HansonServo.ino
View File

@ -17,6 +17,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
#define CMD_DELETE_FILE 0x04
#define CMD_SAVE_FILE 0x05
#define CMD_MESSAGE 0x06
#define CMD_SET_POSITION 0x07
// ESP32 S2 PINOUT
@ -166,6 +167,10 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
handleSaveFile(payload, length);
break;
case CMD_SET_POSITION:
handleSetPosition(payload, length);
break;
default:
Serial.print("Unknown command: ");
Serial.println(command, HEX);
@ -173,7 +178,7 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
}
}
void sendMessage(const String& payload) {
void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
uint16_t length = payload.length();
uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF);
@ -183,14 +188,13 @@ void sendMessage(const String& payload) {
Serial.write(HEADER1);
Serial.write(HEADER2);
Serial.write(CMD_MESSAGE);
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();
@ -309,6 +313,32 @@ void handleLoadFile(const uint8_t* payload, uint16_t length) {
}
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) {
@ -374,8 +404,6 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
char filename[filenameLength + 1];
//strcpy(filename, "/test1.anim");
memcpy(filename, payload + 2, filenameLength);
filename[filenameLength] = '\0';
@ -427,18 +455,47 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
}
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);
@ -451,7 +508,8 @@ void loop() {
// servos.syncWritePos(ids, pos2, 5);
// delay(1000);
if (millis() - lastSend > 1000) {
if (millis() - lastSend > 2000) {
//sendMessageFromESP32(String(millis()));
//handleIdRequest();
//PrintFileList();