Code: Select all
const int txPin = 1; // TX pin of the Arduino connected to the data input of the bus servo controller
const int baudRate = 9600; // baud rate of the bus servo controller
void setup() {
Serial.begin(baudRate);
}
void loop() {
// move servo 1 to position 1000
int servoId = 1;
int position = 1000;
moveServo(servoId, position);
delay(1000);
// move servo 1 to position 2000
position = 2000;
moveServo(servoId, position);
delay(1000);
}
void moveServo(int servoId, int position) {
// prepare the command packet
byte commandPacket[] = {0x55, 0x55, 0x06, 0x03, servoId, position & 0xFF, position >> 8, 0x00};
// calculate the checksum
byte checksum = 0;
for (int i = 2; i < 7; i++) {
checksum ^= commandPacket[i];
}
commandPacket[7] = checksum;
// send the command packet to the servo controller
for (int i = 0; i < sizeof(commandPacket); i++) {
Serial.write(commandPacket[i]);
}
}