Update: replace board, add servo driver program

arduino_code/Test/src/servo_control.cpp

@@ -1,93 +1,98 @@
 #include "servo_control.h"
+#include "mqtt_handler.h"
+#include "config.h"
+
+static Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
+static bool servo_initialized = false;
+
 
-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);
+    Serial.println("Initializing PCA9685 servo driver...");
     
-    servo.setPeriodHertz(50);                   // Стандартные 50 Гц для серво
-    servo.attach(SERVO_PIN, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
-    servo_attached = true;
+    // Инициализация I2C на пинах 21 (SDA) и 22 (SCL)
+    Wire.begin(I2C_SDA_PIN, I2C_SLC_PIN);
     
-    // Устанавливаем стартовое положение
-    servo.write(current_angle);
-    servo_enabled = true;
+    pwm.begin();
+    pwm.setOscillatorFrequency(27000000);
+    pwm.setPWMFreq(50); // 50 Hz для сервоприводов
     
-    Serial.printf("[SERVO] Initialized on pin %d, angle=%d°\n", SERVO_PIN, current_angle);
+    delay(10);
+    
+    servo_initialized = true;
+    Serial.println("PCA9685 initialized successfully");
+    
+    // Инициализируем все каналы как выключенные
+    for (int i = 0; i < MAX_SERVOS; i++) {
+        current_angles[i] = 90; // Начальное положение - середина
+        servo_enabled[i] = false;
+        disableServo(i);
+    }
 }
 
-// ======================================================
-// Управление
-// ======================================================
-void servoSetAngle(int angle) {
-    // Ограничиваем диапазон 0..180
-    if (angle < 0) angle = 0;
+// Преобразование угла (0-180) в длину импульса
+uint16_t angleToPulse(uint8_t angle) {
+    if (angle > 180) angle = 180;
+    return map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
+}
+
+// Преобразование длины импульса в угол
+uint8_t pulseToAngle(uint16_t pulse) {
+    if (pulse < SERVO_MIN_PULSE) return 0;
+    if (pulse > SERVO_MAX_PULSE) return 180;
+    return map(pulse, SERVO_MIN_PULSE, SERVO_MAX_PULSE, 0, 180);
+}
+
+void setServoAngle(uint8_t channel, uint8_t angle) {
+    if (!servo_initialized || channel >= MAX_SERVOS) return;
+    
     if (angle > 180) angle = 180;
     
-    current_angle = angle;
+    current_angles[channel] = angle;
+    servo_enabled[channel] = true;
     
-    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;
+    uint16_t pulse = angleToPulse(angle);
+    setServoPulse(channel, pulse);
     
-    // Ограничиваем диапазон импульса
-    if (pulse_us < SERVO_MIN_PULSE) pulse_us = SERVO_MIN_PULSE;
-    if (pulse_us > SERVO_MAX_PULSE) pulse_us = SERVO_MAX_PULSE;
+    Serial.printf("Servo %d: angle=%d, pulse=%d\n", channel, angle, pulse);
+}
+
+void setServoPulse(uint8_t channel, uint16_t pulse) {
+    if (!servo_initialized || channel >= MAX_SERVOS) return;
     
-    servo.writeMicroseconds(pulse_us);
+    // Преобразование микросекунд в тики PCA9685
+    // PCA9685 имеет 4096 тиков на период при 50Hz = 20000 мкс
+    // 1 мкс = 4096 / 20000 = 0.2048 тика
+    double pulselength = 4096.0 / 20000.0; // тиков на микросекунду
+    uint16_t ticks = pulse * pulselength;
     
-    // Пересчитываем угол для телеметрии (линейная аппроксимация)
-    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);
+    pwm.setPWM(channel, 0, ticks);
 }
 
-void servoEnable(bool enable) {
-    servo_enabled = enable;
+void enableServo(uint8_t channel) {
+    if (!servo_initialized || channel >= MAX_SERVOS) return;
     
-    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)");
-        }
-    }
+    servo_enabled[channel] = true;
+    setServoAngle(channel, current_angles[channel]);
+    
+    Serial.printf("Servo %d: ENABLED\n", channel);
 }
 
-void servoDetach() {
-    if (servo_attached) {
-        servo.detach();
-        servo_attached = false;
-        Serial.println("[SERVO] Detached");
-    }
+void disableServo(uint8_t channel) {
+    if (!servo_initialized || channel >= MAX_SERVOS) return;
+    
+    servo_enabled[channel] = false;
+    pwm.setPWM(channel, 0, 0); // Отключаем сигнал
+    
+    Serial.printf("Servo %d: DISABLED\n", channel);
 }
 
-// ======================================================
-// Получение состояния
-// ======================================================
-int servoGetAngle() {
-    return current_angle;
+uint8_t getServoAngle(uint8_t channel) {
+    if (channel >= MAX_SERVOS) return 0;
+    return current_angles[channel];
 }
 
-bool servoIsEnabled() {
-    return servo_enabled;
-}
-
-bool servoIsAttached() {
-    return servo_attached;
+bool isServoEnabled(uint8_t channel) {
+    if (channel >= MAX_SERVOS) return false;
+    return servo_enabled[channel];
 }