//Read a 6V RC Servo PPM PWM signal with ESP32
//You must use a voltage divider or level shifter on Input pin

#include <Arduino.h>
#include <ESP32Servo.h>
#define PWM_PIN 34
#define SIG_PIN 18
#define STEP 1

Servo servo;
uint32_t nowMicros = 0, pulseWidth, prevTime = 0;
uint8_t printTimer = 0;
int pos = 0, freqCount = 0;
bool direction = 1;

void falling(void){
  attachInterrupt(PWM_PIN, rising, RISING);
  pulseWidth = micros() - nowMicros;
}

void rising(void){
  attachInterrupt(PWM_PIN, falling, FALLING);
  nowMicros = micros();
  freqCount++;
}

void moveServo(){
  if(direction){
    pos += STEP;
    servo.write(pos);
      if(pos > 180){
        direction = false;
        return;
      }
  }else{
    pos -= STEP;
    servo.write(pos);
      if(pos < 0){
        direction = true;
      }
  }
  return;
}

void setup() {
  Serial.begin(115200);
  pinMode(PWM_PIN, INPUT);
  servo.attach(SIG_PIN, 500, 2400);
  attachInterrupt(PWM_PIN, rising, RISING);
  prevTime = millis();
}

void loop() {

  if(millis() - prevTime >= 10){
    printTimer++;

    if(printTimer >= 100){
      printTimer = 0;
      detachInterrupt(PWM_PIN);
      byte readAngle = map(pulseWidth, 500, 2400, 0, 180);
      double pw = ((double)pulseWidth / 1000);
      Serial.printf("  Read Angle: %02i\tRead PWM is %.2lf mS:\n", readAngle, pw);
      Serial.printf("Output Angle: %02i\tFrequency is %i Hz\n\n", pos, freqCount);
     freqCount = 0;
      attachInterrupt(PWM_PIN, rising, RISING);
    }
    moveServo();
    prevTime = millis();
  }
}