309 lines
7.9 KiB
C++
309 lines
7.9 KiB
C++
|
||
bool PRINT_SENT_PACKETS = false;
|
||
#define DE_PIN 20
|
||
#define RE_PIN 10
|
||
#define TX_PIN 21
|
||
#define RX_PIN 9
|
||
|
||
|
||
#define PING 0x01 // QUERY THE WORKING STATUS
|
||
#define READ_DATA 0x02 // READ DATA
|
||
#define WRITE_DATA 0x03 // WRITE DATA
|
||
#define REGWRITE_DATA 0x04 // QUEUES MOVES FOR ACTION COMMAND
|
||
#define ACTION 0x05 // TRIGGERS REG WRITE WRITES
|
||
#define SYNCWRITE_DATA 0x83 // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
|
||
#define RESET 0x06 // RESET TO FACTORY DEFAULT
|
||
|
||
void setup() {
|
||
Serial.begin(115200);
|
||
Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
|
||
pinMode(DE_PIN, OUTPUT);
|
||
pinMode(RE_PIN, OUTPUT);
|
||
SetModeReceive();
|
||
delay(1000);
|
||
}
|
||
|
||
// Send move command to servo id:0-255, position:0-4095
|
||
void sendWritePos(uint8_t id, uint16_t position) {
|
||
uint8_t packet[9];
|
||
|
||
packet[0] = 0xFF; // Header
|
||
packet[1] = 0xFF; // Header
|
||
packet[2] = id; // Servo ID
|
||
packet[3] = 5; // Length = instruction + address + 2 bytes + checksum
|
||
packet[4] = WRITE_DATA; // Instruction: WRITE
|
||
packet[5] = 0x2A; // Address: Goal Position
|
||
packet[6] = position & 0xFF; // Low byte
|
||
packet[7] = (position >> 8) & 0xFF; // High byte
|
||
|
||
// Calculate checksum
|
||
uint8_t sum = 0;
|
||
for (int i = 2; i < 8; i++) sum += packet[i];
|
||
packet[8] = ~sum;
|
||
|
||
// Send packet
|
||
SetModeTransmit();
|
||
Serial1.write(packet, sizeof(packet));
|
||
Serial1.flush();
|
||
SetModeReceive();
|
||
|
||
digitalWrite(DE_PIN, LOW); // Back to receive
|
||
if (PRINT_SENT_PACKETS) {
|
||
Serial.print("Sent packet: ");
|
||
|
||
for (int i = 0; i < 9; i++) {
|
||
Serial.print("0x");
|
||
if (packet[i] < 0x10) Serial.print("0"); // Leading zero for single-digit hex
|
||
Serial.print(packet[i], HEX);
|
||
Serial.print(" ");
|
||
}
|
||
Serial.println();
|
||
}
|
||
}
|
||
|
||
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];
|
||
|
||
packet[0] = 0xFF; // Header
|
||
packet[1] = 0xFF; // Header
|
||
packet[2] = id; // Servo ID
|
||
packet[3] = 0x02; // Length = instruction + address + checksum
|
||
packet[4] = PING; // Instruction: PING
|
||
|
||
// Calculate checksum
|
||
uint8_t sum = 0;
|
||
for (int i = 2; i < 5; i++) sum += packet[i];
|
||
packet[5] = ~sum; // Checksum
|
||
|
||
// Send packet
|
||
SetModeTransmit();
|
||
Serial1.write(packet, sizeof(packet));
|
||
Serial1.flush();
|
||
SetModeReceive();
|
||
|
||
if (PRINT_SENT_PACKETS) {
|
||
Serial.print("Sent ping packet: ");
|
||
for (int i = 0; i < 6; i++) {
|
||
Serial.print("0x");
|
||
if (packet[i] < 0x10) Serial.print("0"); // Leading zero for single-digit hex
|
||
Serial.print(packet[i], HEX);
|
||
Serial.print(" ");
|
||
}
|
||
Serial.println();
|
||
}
|
||
}
|
||
|
||
void WaitOnReply(unsigned long timeout) {
|
||
unsigned long startTime = millis(); // Record the start time
|
||
|
||
while (millis() - startTime < timeout) { // Loop until timeout
|
||
if (Serial1.available()) {
|
||
uint8_t buffer[32];
|
||
int count = 0;
|
||
|
||
// Read all available bytes
|
||
while (Serial1.available() && count < sizeof(buffer)) {
|
||
buffer[count++] = Serial1.read();
|
||
}
|
||
|
||
// Display on Serial
|
||
Serial.print("recv: ");
|
||
Serial.print(buffer[2]);
|
||
Serial.print(" ");
|
||
if (buffer[4] == 0x00) {
|
||
Serial.print("OK");
|
||
} else {
|
||
Serial.print("NOK");
|
||
}
|
||
Serial.print("\t");
|
||
for (int i = 0; i < count; i++) {
|
||
Serial.print("0x");
|
||
if (buffer[i] < 0x10) Serial.print("0");
|
||
Serial.print(buffer[i], HEX);
|
||
Serial.print(" ");
|
||
}
|
||
Serial.println();
|
||
|
||
|
||
break; // Exit the loop after processing the reply
|
||
}
|
||
delay(10); // Small delay to prevent busy-waiting
|
||
}
|
||
}
|
||
|
||
void SetModeTransmit() {
|
||
digitalWrite(DE_PIN, HIGH);
|
||
digitalWrite(RE_PIN, HIGH);
|
||
delay(10);
|
||
}
|
||
|
||
void SetModeReceive() {
|
||
digitalWrite(DE_PIN, LOW);
|
||
digitalWrite(RE_PIN, LOW);
|
||
delay(10);
|
||
}
|
||
|
||
void PingAll() {
|
||
Serial.println("PINGING ALL 0-255");
|
||
for (int i = 0; i < 255; i++) {
|
||
|
||
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);
|
||
} |