ESP, IoT Проблема с движением сервоприводов и мерцанием светодиода

[NSTRV]

Всем привет! Проблема вот в чём: в устройстве два сервопривода Pan, Tilt и rgb светодиод. Скетч писался чатом GPT, переменные редактировал уже сам. Все пины скетча соответствуют реальным на плате. Суть проблемы в том, что сервы резко дёргаются от позиции к позиции с хаотичными задержками, а нужно, чтобы было умеренное, непрерывное движение. Я пока толком не понимаю как это правильно реализовать, поэтому прошу помощи.
Ещё тут есть проблема со светодиодом. Он работает, но не совсем корректно. Смена цвета присутствует, но рывками, как будто бы стробоскопом моргает.
Плата - ESP32-C3
Серво - sg90
Диод - 3.3v, 1w x3
Питание - 5v, 2a + конденсатор на 470uf

Подскажите куда копать и как поправить эти недоразумения.

/*
  NSTRV Mini Moving Head
  Firmware: head v0.3
  Controller: ESP32-C3
  Motors: 2x SG90
  LED: RGB, MOSFET control
  Input: touch sensor, active LOW

  Controls:
  Single tap  - ON / STANDBY
  Double tap  - AUTO / SOUND
  Triple tap  - AUTO SMOOTH / AUTO ACTIVE
  Hold        - brightness up/down, alternating per hold
*/

#include <ESP32Servo.h>

// ---------- PINS ----------

const int PIN_PAN   = 4;
const int PIN_TILT  = 3;

const int PIN_RED   = 6;
const int PIN_GREEN = 7;
const int PIN_BLUE  = 8;

const int PIN_TOUCH = 10;

// ---------- SERVO LIMITS ----------

const int PAN_MIN  = 15;
const int PAN_MAX  = 165;

const int TILT_MIN = 25;
const int TILT_MAX = 150;

const int PAN_HOME  = 90;
const int TILT_HOME = 30;

// If TILT_HOME points upward instead of downward,
// change it to 150 after checking the mechanism.

// ---------- INPUT SETTINGS ----------

const unsigned long DEBOUNCE_MS = 35;
const unsigned long CLICK_WINDOW_MS = 350;
const unsigned long HOLD_START_MS = 500;
const unsigned long BRIGHTNESS_STEP_MS = 45;

// ---------- SERVO SPEED ----------

const unsigned long SERVO_STEP_MS = 1000;

// One degree per step.
// Increase SERVO_STEP_MS for slower movement.

const unsigned long COLOR_STEP_MS = 10;

// ---------- BRIGHTNESS ----------

const int BRIGHTNESS_MIN = 5;
const int BRIGHTNESS_MAX = 100;

int brightness = 100;
int brightnessDirection = 1;

// First hold increases brightness.
// Each subsequent hold reverses direction.

// ---------- OBJECTS ----------

Servo servoPan;
Servo servoTilt;

// ---------- MODES ----------

enum MainMode {
  MODE_AUTO,
  MODE_SOUND
};

enum AutoStyle {
  AUTO_SMOOTH,
  AUTO_ACTIVE
};

MainMode mainMode = MODE_AUTO;
AutoStyle autoStyle = AUTO_SMOOTH;

bool isOn = true;

// ---------- SERVO STATE ----------

int panPosition = PAN_HOME;
int tiltPosition = TILT_HOME;

int panTarget = PAN_HOME;
int tiltTarget = TILT_HOME;

unsigned long lastServoStep = 0;
unsigned long nextMoveTime = 0;

// ---------- RGB STATE ----------

int redLevel = 255;
int greenLevel = 255;
int blueLevel = 255;

int redTarget = 255;
int greenTarget = 0;
int blueTarget = 0;

unsigned long lastColorStep = 0;
unsigned long nextColorTime = 0;

// ---------- TOUCH STATE ----------

bool lastRawTouch = HIGH;
bool stableTouch = HIGH;

unsigned long lastDebounceTime = 0;
unsigned long pressStartTime = 0;
unsigned long lastBrightnessStep = 0;
unsigned long lastReleaseTime = 0;

bool holdProcessed = false;
bool holdDirectionPending = false;

int clickCount = 0;

// ---------- RANDOM HELPERS ----------

int randomBetween(int low, int high) {
  return random(low, high + 1);
}

// ---------- LED CONTROL ----------

void setupLED() {
  // ESP32 Arduino Core 3.x API
  ledcAttach(PIN_RED, 5000, 8);
  ledcAttach(PIN_GREEN, 5000, 8);
  ledcAttach(PIN_BLUE, 5000, 8);
}

void updateLED() {
  if (!isOn) {
    ledcWrite(PIN_RED, 0);
    ledcWrite(PIN_GREEN, 0);
    ledcWrite(PIN_BLUE, 0);
    return;
  }

  int master = map(
    brightness,
    0, 100,
    0, 255
  );

  int r = (redLevel * master) / 255;
  int g = (greenLevel * master) / 255;
  int b = (blueLevel * master) / 255;

  ledcWrite(PIN_RED, r);
  ledcWrite(PIN_GREEN, g);
  ledcWrite(PIN_BLUE, b);
}

void chooseNewColor() {
  // Select one of six saturated RGB colors.
  int color = random(0, 6);

  switch (color) {
    case 0:
      redTarget = 255;
      greenTarget = 0;
      blueTarget = 0;
      break;

    case 1:
      redTarget = 255;
      greenTarget = 100;
      blueTarget = 0;
      break;

    case 2:
      redTarget = 0;
      greenTarget = 255;
      blueTarget = 0;
      break;

    case 3:
      redTarget = 0;
      greenTarget = 255;
      blueTarget = 255;
      break;

    case 4:
      redTarget = 0;
      greenTarget = 0;
      blueTarget = 255;
      break;

    case 5:
      redTarget = 255;
      greenTarget = 0;
      blueTarget = 255;
      break;
  }
}

void updateColor() {
  if (!isOn) return;

  unsigned long now = millis();

  // Choose a new color periodically in AUTO.
  if (mainMode == MODE_AUTO && now >= nextColorTime) {
    chooseNewColor();

    if (autoStyle == AUTO_SMOOTH) {
      nextColorTime = now + randomBetween(3000, 6000);
    } else {
      nextColorTime = now + randomBetween(500, 1600);
    }
  }

  if (now - lastColorStep < COLOR_STEP_MS) return;
  lastColorStep = now;

  // Smooth RGB transitions.
  const int colorStep = 3;

  if (redLevel < redTarget) {
    redLevel = min(redLevel + colorStep, redTarget);
  } else if (redLevel > redTarget) {
    redLevel = max(redLevel - colorStep, redTarget);
  }

  if (greenLevel < greenTarget) {
    greenLevel = min(greenLevel + colorStep, greenTarget);
  } else if (greenLevel > greenTarget) {
    greenLevel = max(greenLevel - colorStep, greenTarget);
  }

  if (blueLevel < blueTarget) {
    blueLevel = min(blueLevel + colorStep, blueTarget);
  } else if (blueLevel > blueTarget) {
    blueLevel = max(blueLevel - colorStep, blueTarget);
  }

  updateLED();
}

// ---------- SERVO CONTROL ----------

void setHomePosition() {
  panTarget = PAN_HOME;
  tiltTarget = TILT_HOME;
}

void chooseMovementTarget() {
  if (!isOn || mainMode == MODE_SOUND) return;

  if (autoStyle == AUTO_SMOOTH) {
    panTarget = randomBetween(PAN_MIN, PAN_MAX);
    tiltTarget = randomBetween(TILT_MIN, TILT_MAX);
  } else {
    panTarget = randomBetween(PAN_MIN, PAN_MAX);
    tiltTarget = randomBetween(TILT_MIN, TILT_MAX);
  }

  nextMoveTime = millis() + 20;
}

void updateServos() {
  if (!isOn || mainMode == MODE_SOUND) {
    // В режиме ожидания движение к домашней позиции
    if (!isOn) {
      panTarget = PAN_HOME;
      tiltTarget = TILT_HOME;
    } else {
      return;
    }
  }

  unsigned long now = millis();

  if (now - lastServoStep < SERVO_STEP_MS) return;
  lastServoStep = now;

  // Плавное движение PAN
  if (panPosition < panTarget) {
    panPosition++;
    servoPan.write(panPosition);
  } else if (panPosition > panTarget) {
    panPosition--;
    servoPan.write(panPosition);
  }

  // Плавное движение TILT
  if (tiltPosition < tiltTarget) {
    tiltPosition++;
    servoTilt.write(tiltPosition);
  } else if (tiltPosition > tiltTarget) {
    tiltPosition--;
    servoTilt.write(tiltPosition);
  }

  // Если оба сервопривода достигли целей,
  // сразу назначаем новые позиции
  if (panPosition == panTarget &&
      tiltPosition == tiltTarget &&
      isOn &&
      mainMode == MODE_AUTO) {
    chooseMovementTarget();
  }
}

// ---------- STANDBY ----------

void enterStandby() {
  isOn = false;

  setHomePosition();

  redTarget = 0;
  greenTarget = 0;
  blueTarget = 0;

  updateLED();
}

void wakeUp() {
  isOn = true;

  mainMode = MODE_AUTO;
  autoStyle = AUTO_SMOOTH;

  setHomePosition();

  chooseNewColor();

  nextMoveTime = millis() + 2000;
  nextColorTime = millis() + 1000;
}

// ---------- BRIGHTNESS ----------

void startBrightnessHold() {
  brightnessDirection = -brightnessDirection;
  holdDirectionPending = false;

  // A pending direction change is handled when
  // a hold is confirmed, not on an ordinary tap.
}

void changeBrightness() {
  brightness += brightnessDirection;

  brightness = constrain(
    brightness,
    BRIGHTNESS_MIN,
    BRIGHTNESS_MAX
  );

  updateLED();
}

// ---------- CLICK ACTIONS ----------

void processClicks() {
  if (clickCount == 0) return;

  if (clickCount == 1) {
    if (isOn) {
      enterStandby();
    } else {
      wakeUp();
    }
  }

  else if (clickCount == 2) {
    if (isOn) {
      if (mainMode == MODE_AUTO) {
        mainMode = MODE_SOUND;
      } else {
        mainMode = MODE_AUTO;
        nextMoveTime = millis();
      }
    }
  }

  else if (clickCount >= 3) {
    if (isOn && mainMode == MODE_AUTO) {
      if (autoStyle == AUTO_SMOOTH) {
        autoStyle = AUTO_ACTIVE;
      } else {
        autoStyle = AUTO_SMOOTH;
      }

      nextMoveTime = millis();
      nextColorTime = millis();
    }
  }

  clickCount = 0;
}

// ---------- TOUCH INPUT ----------

void updateTouch() {
  unsigned long now = millis();
  bool rawTouch = digitalRead(PIN_TOUCH);

  if (rawTouch != lastRawTouch) {
    lastDebounceTime = now;
    lastRawTouch = rawTouch;
  }

  if (now - lastDebounceTime < DEBOUNCE_MS) return;

  if (rawTouch != stableTouch) {
    stableTouch = rawTouch;

    if (stableTouch == LOW) {
      // Touch/press started.
      pressStartTime = now;
      holdProcessed = false;
      lastBrightnessStep = now;
    } else {
      // Touch/press released.
      if (!holdProcessed) {
        clickCount++;
        lastReleaseTime = now;
      }
    }
  }

  // Long press: smoothly change brightness.
  if (stableTouch == LOW &&
      !holdProcessed &&
      now - pressStartTime >= HOLD_START_MS) {

    holdProcessed = true;

    // Cancel taps accumulated before this hold.
    clickCount = 0;

    // Alternate direction on each confirmed hold.
    brightnessDirection = -brightnessDirection;

    lastBrightnessStep = now;
  }

  if (stableTouch == LOW && holdProcessed) {
    if (now - lastBrightnessStep >= BRIGHTNESS_STEP_MS) {
      lastBrightnessStep = now;
      changeBrightness();
    }
  }

  // Wait for the click sequence to finish.
  if (stableTouch == HIGH &&
      clickCount > 0 &&
      now - lastReleaseTime >= CLICK_WINDOW_MS) {
    processClicks();
  }
}

// ---------- SETUP ----------

void setup() {
  Serial.begin(115200);

  randomSeed(micros());

  pinMode(PIN_TOUCH, INPUT_PULLUP);

  setupLED();

  servoPan.setPeriodHertz(50);
  servoTilt.setPeriodHertz(50);

  servoPan.attach(PIN_PAN, 500, 2400);
  servoTilt.attach(PIN_TILT, 500, 2400);

  // Start in the center/home position.
  servoPan.write(PAN_HOME);
  servoTilt.write(TILT_HOME);

  panPosition = PAN_HOME;
  tiltPosition = TILT_HOME;

  isOn = true;
  mainMode = MODE_AUTO;
  autoStyle = AUTO_SMOOTH;

  chooseNewColor();

  nextMoveTime = millis() + 2000;
  nextColorTime = millis() + 1000;

  updateLED();

  Serial.println("NSTRV Mini Moving Head");
  Serial.println("Firmware: head v0.3");
  Serial.println("Mode: AUTO SMOOTH");
}

// ---------- MAIN LOOP ----------

void loop() {
  updateTouch();
  updateServos();
  updateColor();
}