synwrite received, but not sending correct moves
parent
f70677c5cc
commit
df4f7de0ca
|
|
@ -61,6 +61,89 @@ void sendWritePos(uint8_t id, uint16_t position) {
|
|||
}
|
||||
}
|
||||
|
||||
void syncWriteTwoServos(uint8_t id1, uint16_t pos1, uint8_t id2, uint16_t pos2) {
|
||||
const uint8_t SYNC_WRITE = 0x83;
|
||||
const uint8_t START_BYTE = 0xFF;
|
||||
const uint8_t DATA_LEN = 0x05; // 2 bytes for position + 1 byte for speed (optional)
|
||||
|
||||
// Build packet
|
||||
uint8_t packet[] = {
|
||||
START_BYTE, START_BYTE, 0xFE, // Broadcast ID
|
||||
0x0F, // Length = 15 bytes total
|
||||
SYNC_WRITE,
|
||||
0x2A, // Starting address for Goal Position (0x2A)
|
||||
0x04, // Data length per servo (2 bytes pos + 2 bytes speed)
|
||||
id1,
|
||||
(uint8_t)(pos1 & 0xFF), // Position low byte
|
||||
(uint8_t)((pos1 >> 8) & 0xFF),// Position high byte
|
||||
0x00, 0x00, // Speed (optional, set to 0)
|
||||
id2,
|
||||
(uint8_t)(pos2 & 0xFF),
|
||||
(uint8_t)((pos2 >> 8) & 0xFF),
|
||||
0x00, 0x00
|
||||
};
|
||||
|
||||
// Calculate checksum
|
||||
uint8_t checksum = 0;
|
||||
for (int i = 2; i < sizeof(packet); i++) {
|
||||
checksum += packet[i];
|
||||
}
|
||||
checksum = ~checksum;
|
||||
|
||||
// Send packet
|
||||
SetModeTransmit();
|
||||
Serial1.write(packet, sizeof(packet));
|
||||
Serial1.write(checksum);
|
||||
Serial1.flush();
|
||||
SetModeReceive();
|
||||
}
|
||||
void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
|
||||
const uint8_t SYNC_WRITE = 0x83;
|
||||
const uint8_t START_BYTE = 0xFF;
|
||||
const uint8_t BROADCAST_ID = 0xFE;
|
||||
const uint8_t START_ADDR = 0x2A; // Goal Position address
|
||||
const uint8_t DATA_LEN_PER_SERVO = 0x04; // 2 bytes pos + 2 bytes speed
|
||||
|
||||
// Calculate packet length: instruction + address + data length + (count × data per servo)
|
||||
uint8_t packetLen = 4 + (count * (1 + DATA_LEN_PER_SERVO)); // instruction + addr + len + servo data
|
||||
|
||||
// Create packet buffer
|
||||
uint8_t packet[3 + packetLen]; // 3 header bytes + payload
|
||||
uint8_t index = 0;
|
||||
|
||||
// Header
|
||||
packet[index++] = START_BYTE;
|
||||
packet[index++] = START_BYTE;
|
||||
packet[index++] = BROADCAST_ID;
|
||||
packet[index++] = packetLen;
|
||||
packet[index++] = SYNC_WRITE;
|
||||
packet[index++] = START_ADDR;
|
||||
packet[index++] = DATA_LEN_PER_SERVO;
|
||||
|
||||
// Servo data
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
packet[index++] = ids[i];
|
||||
packet[index++] = (uint8_t)(positions[i] & 0xFF); // Low byte
|
||||
packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte
|
||||
packet[index++] = 0x00; // Speed low byte
|
||||
packet[index++] = 0x00; // Speed high byte
|
||||
}
|
||||
|
||||
// Checksum
|
||||
uint8_t checksum = 0;
|
||||
for (uint8_t i = 2; i < index; i++) {
|
||||
checksum += packet[i];
|
||||
}
|
||||
checksum = ~checksum;
|
||||
|
||||
// Send packet
|
||||
SetModeTransmit();
|
||||
Serial1.write(packet, index);
|
||||
Serial1.write(checksum);
|
||||
Serial1.flush();
|
||||
SetModeReceive();
|
||||
}
|
||||
|
||||
// Send a ping command to a specific servo
|
||||
void sendPing(uint8_t id) {
|
||||
uint8_t packet[6];
|
||||
|
|
@ -111,7 +194,7 @@ void WaitOnReply(unsigned long timeout) {
|
|||
Serial.print("recv: ");
|
||||
Serial.print(buffer[2]);
|
||||
Serial.print(" ");
|
||||
if (buffer[4] == 0x00){
|
||||
if (buffer[4] == 0x00) {
|
||||
Serial.print("OK");
|
||||
} else {
|
||||
Serial.print("NOK");
|
||||
|
|
@ -144,17 +227,83 @@ void SetModeReceive() {
|
|||
delay(10);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
void PingAll() {
|
||||
Serial.println("PINGING ALL 0-255");
|
||||
for (int i = 0; i < 255; i++) {
|
||||
|
||||
sendWritePos(2, random(0, 4096));
|
||||
WaitOnReply(1000);
|
||||
delay(1000);
|
||||
sendPing(200);
|
||||
Serial.println("WAITING...");
|
||||
WaitOnReply(1000);
|
||||
delay(1000);
|
||||
sendPing(5);
|
||||
Serial.println("WAITING...");
|
||||
WaitOnReply(1000);
|
||||
delay(1000);
|
||||
sendPing(i);
|
||||
|
||||
WaitOnReply(100);
|
||||
}
|
||||
Serial.println("PINGING COMPLETE");
|
||||
}
|
||||
|
||||
void TestRange(uint8_t id) {
|
||||
for (int i = 0; i < 4095; i += 256) {
|
||||
sendWritePos(id, i);
|
||||
WaitOnReply(100);
|
||||
delay(100);
|
||||
}
|
||||
for (int i = 4095; i > 0; i -= 256) {
|
||||
|
||||
sendWritePos(id, i);
|
||||
WaitOnReply(100);
|
||||
delay(100);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t ids[] = { 0, 4, 10, 15, 40, 41, 43, 44};
|
||||
uint16_t pos[8];
|
||||
|
||||
void loop() {
|
||||
//PingAll();
|
||||
for (int i = 0; i < 4; i += 1) {
|
||||
for (int x = 0; x < 8; x++){
|
||||
pos[x] = i;
|
||||
}
|
||||
syncWriteServos(ids, pos, 8);
|
||||
Serial.println(i);
|
||||
//syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
|
||||
delay(100);
|
||||
}
|
||||
for (int i = 4; i > 0; i -= 8) {
|
||||
for (int x = 0; x < 8; x++){
|
||||
pos[x] = i;
|
||||
}
|
||||
syncWriteServos(ids, pos, 2);
|
||||
Serial.println(i);
|
||||
//syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
|
||||
delay(100);
|
||||
}
|
||||
// TestRange(0);
|
||||
// TestRange(4);
|
||||
// TestRange(10);
|
||||
// TestRange(15);
|
||||
// TestRange(40);
|
||||
// TestRange(41);
|
||||
// TestRange(43);
|
||||
// TestRange(44);
|
||||
|
||||
// sendWritePos(123, 1000);
|
||||
//delay(2000);
|
||||
|
||||
// sendWritePos(123, 2000);
|
||||
// delay(2000);
|
||||
|
||||
|
||||
|
||||
// sendWritePos(34, 4095);
|
||||
// WaitOnReply(50);
|
||||
// delay(100);
|
||||
// sendWritePos(34, 0);
|
||||
// WaitOnReply(50);
|
||||
// delay(100);
|
||||
|
||||
// WaitOnReply(1000);
|
||||
// delay(1000);
|
||||
// delay(1000);
|
||||
// sendPing(5);
|
||||
// Serial.println("WAITING...");
|
||||
// WaitOnReply(1000);
|
||||
// delay(1000);
|
||||
}
|
||||
Loading…
Reference in New Issue