Untitled

/*
управління через PPM
*/

class MotorController {
private:

  const int ppmPin = 2;  // Пін для PPM сигналу
  const int servo_camera_pin = 13; // Пін для сервопрівола

  const int ppmChannels = 8;                                                        // Кількість каналів
  int ppmValues[ppmChannels] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };  // Значення каналів (у мікросекундах)
  const int ppmFrameLength = 22500;                                                 // Тривалість фрейму PPM (у мікросекундах)
  hw_timer_t *timer = NULL;
  Servo servo1;

public:
  MotorController() {}

  void init() {
    pinMode(ppmPin, OUTPUT);
    digitalWrite(ppmPin, LOW);

    // Налаштування таймера
    timer = timerBegin(0, 80, true);  // Таймер 0, на 80 (для 1 мікрос на ESP32 з частотою 80 MHz)
    timerAttachInterrupt(timer, &onTimerISR, true);
    timerAlarmWrite(timer, ppmValues[0] * 2, true);  // Встановлюємо період для першого каналу
    timerAlarmEnable(timer);


    servo1.attach(servo_camera_pin);
    servo1.write(90);
  }

  int scaleValue(int originalValue) {
    return (originalValue * 180) / 200;
  }

  int scaleValueforPPM(int originalValue) {
    return 1000 + ((originalValue * 1000) / 200);
  }

  void move(const uint8_t *data) {

    for (int i = 0; i < ppmChannels; i++) {
      ppmValues[i] = scaleValueforPPM(data[i + 1]);
    }
   if (data[5] == 200) {  // тумблер серво
      servo1.write(scaleValue(data[1]));   
  }

  volatile int ppmCurrentChannel = 0;
  void IRAM_ATTR onTimerISR() {
    if (ppmCurrentChannel == -1) {
      digitalWrite(ppmPin, HIGH);
      timerAlarmWrite(timer, ppmFrameLength - (ppmChannels * 2000), true);
    } else {
      if (ppmCurrentChannel < ppmChannels) {
        digitalWrite(ppmPin, LOW);
        timerAlarmWrite(timer, 300, true);  // Встановлюємо період для паузи
      } else {
        digitalWrite(ppmPin, HIGH);
        timerAlarmWrite(timer, ppmValues[ppmCurrentChannel] * 2, true);
      }
    }

    ppmCurrentChannel++;
    if (ppmCurrentChannel > ppmChannels) {
      ppmCurrentChannel = -1;
    }
  }

  void motors_stop() {
 for (int i = 0; i < 4; i++) {
      ppmValues[i] = 1500;
    }
    // переміщуємо серво камери на місце
    servo1.write(90);
  }
};