// https://forum.arduino.cc/t/control-system-and-ui-control-rc-servo-according-to-incoming-pulses-from-inductive-sensor/1416579

# include <Servo.h>
Servo theServo;

# define thePulse 2  // input pulses on pin 2
# define ON_SW_PIN 3     // do or don't
#define DISPLAY_PERIOD 1000 // Display Info every 1000ms

// as periods in microseconds
const unsigned long minPulse = 34468ul;  // ~30 Hz, low end
const unsigned long maxPulse = 2868ul;   // ~350 Hz, high end

// corresponding servo degrees
const int minDeflection = 5;
const int maxDeflection = 175;

# define PARK   60  // when off, park here

float minFrequency;
float maxFrequency;

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position

void setup() {
  Serial.begin(115200);
  Serial.println("\nCrude frequency -> servo pos.\n");

  theServo.attach(9);  // attaches the servo on pin 9 to the servo object

  pinMode(ON_SW_PIN, INPUT_PULLUP);

  pinMode(thePulse, INPUT_PULLUP);  // Set interrupt pin as input with internal pull-up resistor
  attachInterrupt(digitalPinToInterrupt(thePulse), pulseISR, RISING);

  minFrequency = 1.0 / maxPulse;
  maxFrequency = 1.0 / minPulse;
}

volatile unsigned long edgeToEdge;

// each rising edge triggers this function
void pulseISR() {
  static unsigned long lastEdge;
  unsigned long thisEdge;

  thisEdge = micros();
  edgeToEdge = thisEdge - lastEdge;

  lastEdge = thisEdge;
}

// a little Algebra
float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void loop() {
  if (digitalRead(ON_SW_PIN) == LOW) {
    theServo.write(PARK);
  }
  else {
    // else... check if there's a plausible measurement and move the servo accordianly
    unsigned long now = millis();  // the time for this entire pass

    noInterrupts();
    unsigned long myE2E = edgeToEdge;
    interrupts();

    if (maxPulse <= myE2E && myE2E <= minPulse) {
      float frequency = 1.0 / myE2E;
      int deflection = fmap(frequency, minFrequency, maxFrequency, minDeflection, maxDeflection);
      theServo.write(deflection);
    }

    // periodically print the edgeToEdge value
    // as a check
    static unsigned long nextCheck = 0;
    if (now > nextCheck) {
      Serial.println(myE2E);
      nextCheck += DISPLAY_PERIOD;
    }
  }
}