From 23731677851c16aa1373944b075616ab7a2d82bb Mon Sep 17 00:00:00 2001
From: MangaValk <patrickvalkmanga@hotmail.com>
Date: Sun, 12 May 2019 18:30:56 +0200
Subject: [PATCH] Teensy 2019: read SSC-32 style servo commands, first step to
 replacing the SSC-32

From the Arduino sketchbook.
---
 .../hexapod_ssc32_emulator.ino                | 41 +++++++++++++++++++
 1 file changed, 41 insertions(+)
 create mode 100644 2019-teensy-nrf24/tests/hexapod_ssc32_emulator/hexapod_ssc32_emulator.ino

diff --git a/2019-teensy-nrf24/tests/hexapod_ssc32_emulator/hexapod_ssc32_emulator.ino b/2019-teensy-nrf24/tests/hexapod_ssc32_emulator/hexapod_ssc32_emulator.ino
new file mode 100644
index 0000000..2f31a99
--- /dev/null
+++ b/2019-teensy-nrf24/tests/hexapod_ssc32_emulator/hexapod_ssc32_emulator.ino
@@ -0,0 +1,41 @@
+float servos[18];
+
+void setup() {
+  Serial.println("servo serial control");
+}
+
+
+void loop() // "1_180 2_90"
+{
+  if (Serial.available())
+  {
+    String data = Serial.readStringUntil('\r');
+    int maxIndex = data.length() - 1;
+    int index = 0;
+    int next_index;
+    String data_word;
+    String channel;
+    String angle;
+    do {
+      next_index = data.indexOf(',', index);
+
+      data_word = data.substring(index, next_index);
+      channel = data_word.substring(0, data_word.indexOf('_'));
+      angle = data_word.substring(data_word.indexOf('_') + 1);
+
+      servos[channel.toInt()] = angle.toFloat();
+
+      index = next_index + 1;
+    } while ((next_index != -1) && (next_index < maxIndex));
+
+    Serial.println("Output line: ");
+    for (int i = 0; i < 18; i++)
+    {
+      Serial.print(i);
+      Serial.print(", ");
+      Serial.print(servos[i]);
+      Serial.println("");
+    }
+    Serial.println("");
+  }
+}