add servo

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();
+}

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();

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();

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;
+}

src/servo.h

@@ -0,0 +1,6 @@
+#pragma once
+
+void servoInit();
+void servoCenter();
+void servoSetAngle(int angle);
+int  servoGetAngle();

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);