From cb4d09b426a942b540b54458cbf0d08ff6beb6dd Mon Sep 17 00:00:00 2001
From: unknown <you@example.com>
Date: Fri, 23 Jan 2026 00:35:06 +0300
Subject: [PATCH] add servo

---
 src/controller.cpp | 13 ++++++++++
 src/controller.h   |  2 ++
 src/main.cpp       |  2 ++
 src/servo.cpp      | 59 ++++++++++++++++++++++++++++++++++++++++++++++
 src/servo.h        |  6 +++++
 src/ws_server.cpp  | 13 ++++++++++
 6 files changed, 95 insertions(+)
 create mode 100644 src/servo.cpp
 create mode 100644 src/servo.h

diff --git a/src/controller.cpp b/src/controller.cpp
index 372116e..08b9f36 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -1,6 +1,7 @@
 #include "controller.h"
 #include "actuators.h"
 #include "config.h"
+#include "servo.h"
 
 void controllerSetMode(RobotMode m) {
   robot.mode = m;
@@ -37,3 +38,15 @@ void controllerUpdate() {
     controllerStop();
   }
 }
+
+void controllerSetServo(int angle) {
+  // серво разрешено двигать ТОЛЬКО в MANUAL или SERVICE
+  if (robot.mode != MODE_MANUAL && robot.mode != MODE_SERVICE) return;
+
+  servoSetAngle(angle);
+  robot.lastCmdMs = millis();
+}
+
+void controllerCenterServo() {
+  servoCenter();
+}
diff --git a/src/controller.h b/src/controller.h
index eade9b0..b5eb0e8 100644
--- a/src/controller.h
+++ b/src/controller.h
@@ -7,3 +7,5 @@ bool controllerMove(const String& cmd);
 void controllerStop();
 void controllerSetSpeed(int l, int r);
 void controllerUpdate();
+void controllerSetServo(int angle);
+void controllerCenterServo();
diff --git a/src/main.cpp b/src/main.cpp
index d7970e5..512cf82 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -5,10 +5,12 @@
 #include "controller.h"
 #include "robot_state.h"
 #include "ws_server.h"
+#include "servo.h"
 
 void setup() {
   Serial.begin(115200);
 
+  servoInit();
   actuatorsInit();
   webServerInit();
   wsInit();
diff --git a/src/servo.cpp b/src/servo.cpp
new file mode 100644
index 0000000..660245a
--- /dev/null
+++ b/src/servo.cpp
@@ -0,0 +1,59 @@
+#include <Arduino.h>
+#include "servo.h"
+
+// ===== SERVO CONFIG =====
+#define SERVO_PIN 27
+#define SERVO_CH  4
+
+const uint32_t servoFreq = 50;
+const uint8_t  servoRes  = 16;
+
+// PWM диапазон (у тебя уже рабочий)
+const uint32_t SERVO_MIN = 1638;
+const uint32_t SERVO_MAX = 8192;
+
+// ===== OFFSET CALIBRATION =====
+// ПОЛОЖИТЕЛЬНОЕ значение → вправо
+// ОТРИЦАТЕЛЬНОЕ → влево
+const int SERVO_OFFSET_DEG = -6;   // ← подбирается опытно
+
+// логические пределы
+const int SERVO_LOGICAL_MIN = 0;
+const int SERVO_LOGICAL_MAX = 180;
+
+static int currentAngle = 90;
+
+// угол → duty
+static uint32_t angleToDuty(int logicalAngle) {
+  // применяем offset
+  int calibratedAngle = logicalAngle + SERVO_OFFSET_DEG;
+
+  // защита от выхода за пределы
+  calibratedAngle = constrain(calibratedAngle,
+                               SERVO_LOGICAL_MIN,
+                               SERVO_LOGICAL_MAX);
+
+  return map(calibratedAngle,
+             SERVO_LOGICAL_MIN, SERVO_LOGICAL_MAX,
+             SERVO_MIN, SERVO_MAX);
+}
+
+void servoInit() {
+  ledcSetup(SERVO_CH, servoFreq, servoRes);
+  ledcAttachPin(SERVO_PIN, SERVO_CH);
+  servoCenter();
+}
+
+void servoCenter() {
+  currentAngle = 90;
+  ledcWrite(SERVO_CH, angleToDuty(currentAngle));
+}
+
+void servoSetAngle(int angle) {
+  currentAngle = constrain(angle, SERVO_LOGICAL_MIN, SERVO_LOGICAL_MAX);
+  ledcWrite(SERVO_CH, angleToDuty(currentAngle));
+}
+
+int servoGetAngle() {
+  return currentAngle;
+}
diff --git a/src/servo.h b/src/servo.h
new file mode 100644
index 0000000..a2b31c7
--- /dev/null
+++ b/src/servo.h
@@ -0,0 +1,6 @@
+#pragma once
+
+void servoInit();
+void servoCenter();
+void servoSetAngle(int angle);
+int  servoGetAngle();
diff --git a/src/ws_server.cpp b/src/ws_server.cpp
index 0da44f7..33ce978 100644
--- a/src/ws_server.cpp
+++ b/src/ws_server.cpp
@@ -40,6 +40,19 @@ void wsEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
         }
       }
 
+      else if (msg.startsWith("SERVO:")) { //ws.send("SERVO:90")
+        Serial.println(msg);
+        String val = msg.substring(6);
+        val.toUpperCase();
+
+        if (val == "CENTER") {
+            controllerCenterServo();
+        } else {
+            int angle = val.toInt();   // "90" → 90
+            controllerSetServo(angle);
+            }
+        }
+
       else if (msg.startsWith("SPEED:")) {
         int v = msg.substring(6).toInt();
         controllerSetSpeed(v, v);