From 5c2ef77b36b5416f879b510893fda5dae0191260 Mon Sep 17 00:00:00 2001
From: Wang Zhiyu <david-ajax@outlook.com>
Date: Tue, 12 Aug 2025 18:34:42 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=20sample.ino?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 sample.ino | 112 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 112 insertions(+)
 create mode 100644 sample.ino

diff --git a/sample.ino b/sample.ino
new file mode 100644
index 0000000..25ba6a3
--- /dev/null
+++ b/sample.ino
@@ -0,0 +1,112 @@
+#include <Servo.h>
+#include <Arduino_JSON.h>
+#include <Arduino_AVRSTL.h>
+#include <assert.h>
+#include <map>
+#include <String>
+
+constexpr short svo_num = 2;
+constexpr short svo_dig_pin[svo_num] = {9, 10};
+Servo svo[svo_num]; // al, ar, tl, tr
+short svo_angle[svo_num];
+std::map<String, short> svo_name2idx = {{"al", 0}, {"ar", 1}};
+
+constexpr short eng_num = 1;
+constexpr short eng_dig_pin[eng_num] = {11};
+Servo eng[eng_num]; // al, ar, tl, tr
+short eng_speed[eng_num];
+std::map<String, short> eng_name2idx = {{"e1", 0}};
+JSONVar data;
+
+void svo_set(short idx, short angle){
+    svo[idx].write(angle);
+    svo_angle[idx] = angle;
+    delay(15);
+}
+
+short svo_get(short idx){
+    return svo_angle[idx];
+}
+
+void svo_pnt_by_name(String name){
+    Serial.print("{\"" + name + "\":");
+    Serial.print(svo_angle[svo_name2idx[name]]);
+    Serial.print("}\r\n");
+}
+
+void svo_set_by_name(String name, short num){
+    svo_set(svo_name2idx[name], num);
+}
+
+void eng_set(short idx, short speed){
+    eng[idx].writeMicroseconds(speed);
+    eng_speed[idx] = speed;
+    delay(15);
+}
+
+short eng_get(short idx){
+    return eng_speed[idx];
+}
+
+void eng_pnt_by_name(String name){
+    Serial.print("{\"" + name + "\":");
+    Serial.print(eng_speed[eng_name2idx[name]]);
+    Serial.print("}\r\n");
+}
+
+void eng_set_by_name(String name, short num){
+    eng_set(eng_name2idx[name], num);
+}
+
+void setup() {
+    for(short i = 0; i < svo_num; ++i){
+        svo[i].attach(svo_dig_pin[i]);
+    }
+    for(short i = 0; i < svo_num; ++i){
+        svo_set(i, 0);
+    }
+    for(short i = 0; i < eng_num; ++i){
+        eng[i].attach(eng_dig_pin[i]);
+    }
+    for(short i = 0; i < eng_num; ++i){
+        eng_set(i, 0);
+    }
+    Serial.begin(9600);
+    //Serial.println("\r\n");
+}
+void handle(String type, String device, String option, short num){
+    if(type == "svo"){
+        if(option == "q"){
+            svo_pnt_by_name(device);
+        }
+        if(option == "s"){
+            svo_set_by_name(device, num);
+        }
+    }
+    if(type == "eng"){
+        if(option == "q"){
+            eng_pnt_by_name(device);
+        }
+        if(option == "s"){
+            eng_set_by_name(device, num);
+        }
+    }
+}
+
+void loop() {
+    //Serial.print("hihihi\r\n");
+    //Serial.print("\r\n");
+    while(Serial.available() == 0);
+    data = JSON.parse(Serial.readString());
+    if(JSON.typeof(data) == "undefined") {
+        Serial.print("json_failed\r\n");
+        return;
+    }
+    handle(data["type"], data["device"], data["option"], data["num"]);
+    /*
+        data["t(ype)"]: svo/eng
+        data["d(evice)"]: al, ar, tl, tr
+        data["o(ption)"]: s(et)/q(uery)
+        data["n(um)"]: SHORT
+    */
+}