New: adding control servo-drive

2026-07-07 21:54:03 +07:00
parent b69fe70122
commit 023e699771
6 changed files with 270 additions and 15 deletions
--- a/arduino_code/Test/src/servo_control.cpp
+++ b/arduino_code/Test/src/servo_control.cpp
@@ -0,0 +1,93 @@
+#include "servo_control.h"
+
+static Servo servo;
+static int current_angle = 90;          // Стартовое положение — центр
+static bool servo_enabled = false;
+static bool servo_attached = false;
+
+// ======================================================
+// Инициализация
+// ======================================================
+void servoInit() {
+    // Разрешаем таймеры ESP32 для работы с серво
+    // ESP32PWM::allocateTimer(0);
+    // ESP32PWM::allocateTimer(1);
+    
+    servo.setPeriodHertz(50);                   // Стандартные 50 Гц для серво
+    servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
+    servo_attached = true;
+    
+    // Устанавливаем стартовое положение
+    servo.write(current_angle);
+    servo_enabled = true;
+    
+    Serial.printf("[SERVO] Initialized on pin %d, angle=%d°\n", SERVO_PIN, current_angle);
+}
+
+// ======================================================
+// Управление
+// ======================================================
+void servoSetAngle(int angle) {
+    // Ограничиваем диапазон 0..180
+    if (angle < 0) angle = 0;
+    if (angle > 180) angle = 180;
+    
+    current_angle = angle;
+    
+    if (servo_attached && servo_enabled) {
+        servo.write(current_angle);
+    }
+    Serial.printf("[SERVO] Angle set to %d°\n", current_angle);
+}
+
+void servoSetPulse(uint16_t pulse_us) {
+    if (!servo_attached) return;
+    
+    // Ограничиваем диапазон импульса
+    if (pulse_us < SERVO_MIN_PULSE) pulse_us = SERVO_MIN_PULSE;
+    if (pulse_us > SERVO_MAX_PULSE) pulse_us = SERVO_MAX_PULSE;
+    
+    servo.writeMicroseconds(pulse_us);
+    
+    // Пересчитываем угол для телеметрии (линейная аппроксимация)
+    current_angle = map(pulse_us, SERVO_MIN_PULSE, SERVO_MAX_PULSE, 0, 180);
+    Serial.printf("[SERVO] Pulse set to %d µs (~%d°)\n", pulse_us, current_angle);
+}
+
+void servoEnable(bool enable) {
+    servo_enabled = enable;
+    
+    if (servo_attached) {
+        if (servo_enabled) {
+            servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
+            servo.write(current_angle);
+            Serial.println("[SERVO] Enabled");
+        } else {
+            servo.detach();
+            Serial.println("[SERVO] Disabled (detached)");
+        }
+    }
+}
+
+void servoDetach() {
+    if (servo_attached) {
+        servo.detach();
+        servo_attached = false;
+        Serial.println("[SERVO] Detached");
+    }
+}
+
+// ======================================================
+// Получение состояния
+// ======================================================
+int servoGetAngle() {
+    return current_angle;
+}
+
+bool servoIsEnabled() {
+    return servo_enabled;
+}
+
+bool servoIsAttached() {
+    return servo_attached;
+}