34 lines
613 B
C++
34 lines
613 B
C++
/*
|
|
Ping指令测试,测试总线上相应ID舵机是否就绪,广播指令只适用于总线只有一个舵机情况
|
|
*/
|
|
|
|
#include <SCServo.h>
|
|
|
|
SCSCL sc;
|
|
|
|
int LEDpin = 13;
|
|
void setup()
|
|
{
|
|
pinMode(LEDpin,OUTPUT);
|
|
digitalWrite(LEDpin, HIGH);
|
|
Serial.begin(115200);
|
|
Serial1.begin(1000000);
|
|
sc.pSerial = &Serial1;
|
|
delay(1000);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
int ID = sc.Ping(1);
|
|
if(!sc.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);
|
|
}
|
|
}
|