35 lines
693 B
C++
35 lines
693 B
C++
/*
|
|
Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况
|
|
*/
|
|
|
|
#include <SCServo.h>
|
|
|
|
HLSCL hlscl;
|
|
|
|
int LEDpin = 13;
|
|
void setup()
|
|
{
|
|
pinMode(LEDpin,OUTPUT);
|
|
digitalWrite(LEDpin, HIGH);
|
|
Serial.begin(115200);
|
|
//Serial1.begin(1000000, SERIAL_8N1, 18, 17);//esp32-s3
|
|
Serial1.begin(1000000);//mega2560
|
|
hlscl.pSerial = &Serial1;
|
|
delay(1000);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
int ID = hlscl.Ping(1);
|
|
if(!hlscl.getLastError()){
|
|
digitalWrite(LEDpin, LOW);
|
|
Serial.print("Servo ID:");
|
|
Serial.println(ID, DEC);
|
|
delay(100);
|
|
}else{
|
|
Serial.println("Ping servo ID error!");
|
|
digitalWrite(LEDpin, HIGH);
|
|
delay(2000);
|
|
}
|
|
}
|