// Can a Arduino Uno read its own servo signal ?
// Start the simulation and turn the potentiometer with your mouse.
//
//
// Code to read the Servo signal from:
//   https://www.camelsoftware.com/2015/12/25/reading-pwm-signals-from-an-rc-receiver-with-arduino/
//   I made a few changes, it was not a working sketch anyway.
//
// The Arduino Uno micros() function has a 4 µs resolution.
// So I added some smoothing with a walking average:
//   https://www.arduino.cc/en/Tutorial/BuiltInExamples/Smoothing
//
//
// Major flaw of this sketch:
//   It does not detect if there is no servo signal
//
 
#include <Servo.h>

Servo myServo;
const int servoPin = 10;            // pin for servo output

const int servoInputPin = 2;        // to read the servo signal, must be interrupt pin

unsigned long timer_start;          // the timestamp of the rising edge
volatile unsigned long pulse_time;  // the timing of the servo signal
bool timerRunning;                  // eliminates false value for first pulse

// Walking average smoothing filter
// A 16-bit unsinged int is good enough for the servo pulse duration
const int numReadings = 15;         // lower is more jitter, higher is slower
unsigned int readings[numReadings]; // the readings from the analog input
int readIndex = 0;                  // the index of the current reading
unsigned int total = 0;             // the running total


// calcSignal is the interrupt handler
void calcSignal() 
{
  if( digitalRead( servoInputPin) == HIGH)    // signal just went up ?
  { 
    timer_start = micros();
    timerRunning = true;
  } 
  else
  { 
    if( timerRunning)               // signal went down and timer is active ?
    { 
      pulse_time = micros() - timer_start;
      timerRunning = false;
    }
  } 
} 
 

void setup() 
{
    Serial.begin( 115200);
    Serial.println( "The sketch has started");
    myServo.attach( servoPin);

    attachInterrupt( digitalPinToInterrupt( servoInputPin), calcSignal, CHANGE);
} 
 
void loop()
{
  int pot = analogRead( A0);
  int angle = map( pot, 0, 1023, 0, 180);
  myServo.write( angle);

  // The 'pulse_time' is calculated in a interrupt routine.
  // To be sure that the whole value is taken, the interrupts are turned off.
  // The copy of that value is used in the sketch.
  // Warning: 
  //    1. The pulse_time could be zero when no interrupt occured yet.
  //    2. It is not detected when the input signal has stopped.
  noInterrupts();
  unsigned long pulse_time_copy = pulse_time;
  interrupts();

  Serial.print( pulse_time_copy);        // raw value: new micros() - old micros()
  Serial.print( ", ");


  // Running Average (Smoothing) filter
  // subtract the last reading:
  total = total - readings[readIndex];
  // read from the sensor:
  readings[readIndex] = (unsigned int) pulse_time_copy;
  // add the reading to the total:
  total = total + readings[readIndex];
  // advance to the next position in the array and wrap around if needed
  readIndex++;
  if (readIndex >= numReadings) 
  {
    readIndex = 0;
  }

  // calculate the average:
  unsigned int average = total / numReadings;
  Serial.println(average);

  delay(40);
}