Bus Scanner
This sketch scans the bus for servos, printing out the addresses (in hex!) as it finds them. Great for debugging!
Tip: The default address of DRS-0101 servos is 0xFD
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <HerkulexServo.h>
#define PIN_SW_RX 8
#define PIN_SW_TX 9
SoftwareSerial servo_serial(PIN_SW_RX, PIN_SW_TX);
HerkulexServoBus herkulex_bus(servo_serial);
void setup() {
Serial.begin(115200);
servo_serial.begin(115200);
delay(500);
Serial.println("Press 's' to scan the bus for servos.");
}
void loop() {
herkulex_bus.update();
if (Serial.available() > 0) {
char c = Serial.read();
if (c == 's') {
Serial.println("Scanning...");
Serial.println("Addresses are displayed in hexadecimal");
uint8_t servos_found = 0;
for (uint8_t id = 0; id <= 0xFD; id++) {
HerkulexPacket resp;
bool success = herkulex_bus.sendPacketAndReadResponse(resp, id, HerkulexCommand::Stat);
if (success) {
servos_found++;
if (id <= 0x0F) {
Serial.print("0");
}
Serial.print(id, HEX);
} else {
Serial.print("--");
}
if ( ( (id+1) % 0x0F) == 0) {
Serial.println();
} else {
Serial.print(" ");
}
}
Serial.println();
Serial.println("Done!");
Serial.print("Found ");
Serial.print(servos_found);
Serial.println(" servos.");
}
}
}