diff --git a/EspSoftwareSerial/examples/servoTester/servoTester.ino b/EspSoftwareSerial/examples/servoTester/servoTester.ino deleted file mode 100644 index cbc784d8..00000000 --- a/EspSoftwareSerial/examples/servoTester/servoTester.ino +++ /dev/null @@ -1,115 +0,0 @@ -#include -#include - -SoftwareSerial swSer; - -byte buf[10] = { 0xFA, 0xAF,0x00,0x00,0x00, 0x00, 0x00, 0x00, 0x00, 0xED }; -byte cmd[10] = { 0xFA, 0xAF,0x00,0x00,0x00, 0x00, 0x00, 0x00, 0x00, 0xED }; -byte ver[10] = { 0xFC, 0xCF,0x00,0xAA,0x41, 0x16, 0x51, 0x01, 0x00, 0xED }; - - -void setup() { - delay(2000); - Serial.begin(115200); - Serial.println("\nAlpha 1S Servo Tester"); - swSer.begin(115200, SWSERIAL_8N1, 12, 12, false, 256); -} - -void loop() { - for (int i = 1; i <= 32; i++) { - GetVersion(i); - delay(100); - } - SetLED(1, 0); - GoPos(1, 0, 50); - delay(1000); - GoPos(1, 90, 50); - delay(1000); - GoPos(1, 100, 50); - delay(1000); - SetLED(1, 1); - delay(2000); -} - - - - -void GetVersion(byte id) { - memcpy(buf, cmd, 10); - buf[0] = 0xFC; - buf[1] = 0xCF; - buf[2] = id; - buf[3] = 0x01; - SendCommand(); -} - - -void GoPos(byte id, byte Pos, byte Time) { - memcpy(buf, cmd, 10); - buf[2] = id; - buf[3] = 0x01; - buf[4] = Pos; - buf[5] = Time; - buf[6] = 0x00; - buf[7] = Time; - SendCommand(); -} - -void GetPos(byte id) { - memcpy(buf, cmd, 10); - buf[2] = id; - buf[3] = 0x02; - SendCommand(); -} - - -void SetLED(byte id, byte mode) { - memcpy(buf, cmd, 10); - buf[2] = id; - buf[3] = 0x04; - buf[4] = mode; - SendCommand(); -} - -void SendCommand() { - SendCommand(true); -} - -void SendCommand(bool checkResult) { - byte sum = 0; - for (int i = 2; i < 8; i++) { - sum += buf[i]; - } - buf[8] = sum; - ShowCommand(); - swSer.flush(); - swSer.enableTx(true); - swSer.write(buf, 10); - swSer.enableTx(false); - if (checkResult) checkReturn(); -} - -void ShowCommand() { - Serial.print(millis()); - Serial.print(" OUT>>"); - for (int i = 0; i < 10; i++) { - Serial.print((buf[i] < 0x10 ? " 0" : " ")); - Serial.print(buf[i], HEX); - } - Serial.println(); -} - -void checkReturn() { - unsigned long startMs = millis(); - while (((millis() - startMs) < 500) && (!swSer.available())); - if (swSer.available()) { - Serial.print(millis()); - Serial.print(" IN>>>"); - while (swSer.available()) { - byte ch = (byte)swSer.read(); - Serial.print((ch < 0x10 ? " 0" : " ")); - Serial.print(ch, HEX); - } - Serial.println(); - } -}