155 lines
3.8 KiB
C++
155 lines
3.8 KiB
C++
#include <Wire.h>
|
|
#include <Adafruit_GFX.h>
|
|
#include <Adafruit_SSD1306.h>
|
|
|
|
// OLED setup
|
|
#define SCREEN_WIDTH 128
|
|
#define SCREEN_HEIGHT 64
|
|
#define OLED_ADDRESS 0x3C
|
|
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
|
|
|
|
// RS485 control pins
|
|
#define DE_PIN 20
|
|
#define RE_PIN 10
|
|
|
|
#define THIS_ID 200
|
|
#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() {
|
|
// Initialize Serial1 for RS485 (TX=21, RX=9)
|
|
Serial.begin(115200);
|
|
Serial1.begin(1000000, SERIAL_8N1, 9, 21);
|
|
|
|
// Set DE and RE as outputs
|
|
pinMode(DE_PIN, OUTPUT);
|
|
pinMode(RE_PIN, OUTPUT);
|
|
|
|
SetModeReceive();
|
|
|
|
// Initialize OLED
|
|
Wire.begin(3, 4); // SDA=3, SCL=4
|
|
if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
|
|
while (true)
|
|
; // Halt if display fails
|
|
}
|
|
display.clearDisplay();
|
|
display.setTextSize(1);
|
|
display.setTextColor(SSD1306_WHITE);
|
|
display.setCursor(0, 0);
|
|
display.println("Waiting for RS485...");
|
|
display.display();
|
|
}
|
|
|
|
void loop() {
|
|
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: ");
|
|
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();
|
|
|
|
// Display on OLED
|
|
display.clearDisplay();
|
|
display.setCursor(0, 0);
|
|
display.setTextSize(1);
|
|
display.println("Received:");
|
|
display.setCursor(0, 12);
|
|
display.setTextSize(1);
|
|
if (buffer[4] == PING) {
|
|
if (buffer[2] == THIS_ID){
|
|
sendPingResponse();
|
|
Serial.println("ME!");
|
|
display.print("!");
|
|
}
|
|
Serial.println("PING");
|
|
|
|
display.print("PING");
|
|
display.print(" ");
|
|
display.println(buffer[2]);
|
|
} else if (buffer[4] == WRITE_DATA) {
|
|
Serial.println("WRITE_DATA");
|
|
uint16_t position = (buffer[7] << 8) | buffer[6];
|
|
display.print("WRITE_DATA");
|
|
display.print(" ");
|
|
display.print(buffer[2]);
|
|
display.print(" ");
|
|
display.println(position);
|
|
}
|
|
|
|
for (int i = 0; i < count; i++) {
|
|
display.print("0x");
|
|
if (buffer[i] < 0x10) display.print("0");
|
|
display.print(buffer[i], HEX);
|
|
display.print(" ");
|
|
}
|
|
|
|
|
|
|
|
display.display();
|
|
}
|
|
}
|
|
|
|
|
|
void SetModeTransmit() {
|
|
digitalWrite(DE_PIN, HIGH);
|
|
digitalWrite(RE_PIN, HIGH);
|
|
delay(10);
|
|
}
|
|
|
|
void SetModeReceive() {
|
|
digitalWrite(DE_PIN, LOW);
|
|
digitalWrite(RE_PIN, LOW);
|
|
delay(10);
|
|
}
|
|
|
|
// Send a ping command to a specific servo
|
|
void sendPingResponse() {
|
|
uint8_t packet[6];
|
|
|
|
packet[0] = 0xFF; // Header
|
|
packet[1] = 0xFF; // Header
|
|
packet[2] = THIS_ID; // Servo ID
|
|
packet[3] = 0x03; // Length = instruction + address + checksum
|
|
packet[4] = 0x00; // NO ERROR
|
|
|
|
// 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();
|
|
// }
|
|
}
|