From 6de4a33e5c7b35594e27e35c119a5383eb0e214c Mon Sep 17 00:00:00 2001
From: Luthics <luthics@foxmail.com>
Date: Sat, 12 Nov 2022 19:36:48 +0800
Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E8=88=B5=E6=9C=BA=E5=BA=93?=
 =?UTF-8?q?=EF=BC=8C=E5=A4=96=E6=94=BE=E5=87=BD=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 include/servo.h |  3 ++-
 src/bt.cpp      |  1 +
 src/servo.cpp   | 61 +++++++++++++++++++++++++++++++++++--------------
 3 files changed, 47 insertions(+), 18 deletions(-)

diff --git a/include/servo.h b/include/servo.h
index c05ba55..b26dc7d 100644
--- a/include/servo.h
+++ b/include/servo.h
@@ -1,2 +1,3 @@
 void servo_init();
-void servo_loop();
\ No newline at end of file
+void servo_loop();
+void servo_deg(int servoid, int deg);
\ No newline at end of file
diff --git a/src/bt.cpp b/src/bt.cpp
index 320dbf6..ead9d02 100644
--- a/src/bt.cpp
+++ b/src/bt.cpp
@@ -1,4 +1,5 @@
 #include <NimBLEDevice.h>
+#include <bt.h>
 
 #define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
 #define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"
diff --git a/src/servo.cpp b/src/servo.cpp
index cd84186..202e66c 100644
--- a/src/servo.cpp
+++ b/src/servo.cpp
@@ -1,5 +1,6 @@
 #include <Arduino.h>
 #include <ESP32Servo.h>
+#include <servo.h>
 
 Servo servo[8];
 ESP32PWM pwm;
@@ -16,29 +17,55 @@ void servo_init() {
     ESP32PWM::allocateTimer(i);
     servo[i].setPeriodHertz(50);
   }
-}
-
-void servo_loop() {
   for (int i = 0; i < 8; i++) {
     servo[i].attach(servoPin[i], minUs, maxUs);
   }
   pwm.attachPin(27, 10000);
+}
 
-  //   for (int j = 0; j < 8; j++) {
-  //     for (pos = 0; pos <= 180; pos += 1) {
-  //       servo[j].write(pos);
-  //       Serial.println(pos);
-  //       delay(5);
-  //     }
-  //     for (pos = 0; pos <= 180; pos += 1) {
-  //       servo[j].write(180 - pos);
-  //       Serial.println(180 - pos);
-  //       delay(5);
-  //     }
+void servo_loop() {
+  // servo_deg(8, 0);
+  // delay(5000);
+  // servo_deg(8, 180);
+  // delay(5000);
+  // for (int i = 0; i < 8; i++) {
+  //   servo[i].attach(servoPin[i], minUs, maxUs);
+  // }
+  // pwm.attachPin(27, 10000);
+
+  // for (int j = 0; j < 8; j++) {
+  //   for (pos = 0; pos <= 180; pos += 1) {
+  //     servo[j].write(pos);
+  //     Serial.println(pos);
+  //     delay(5);
   //   }
+  //   for (pos = 0; pos <= 180; pos += 1) {
+  //     servo[j].write(180 - pos);
+  //     Serial.println(180 - pos);
+  //     delay(5);
+  //   }
+  // }
 
-  for (int i = 0; i < 8; i++) {
-    servo[i].detach();
+  // for (int i = 0; i < 8; i++) {
+  //   servo[i].detach();
+  // }
+  // pwm.detachPin(27);
+}
+
+void servo_deg(int servoid, int deg) {
+  if (deg < 0) {
+    deg = 0;
+  }
+  if (deg > 180) {
+    deg = 180;
+  }
+  if (servoid >= 8) {
+    for (int i = 0; i < 8; i++) {
+      servo[i].write(deg);
+      Serial.println(deg);
+    }
+  } else {
+    servo[servoid].write(deg);
+    Serial.println(deg);
   }
-  pwm.detachPin(27);
 }
\ No newline at end of file