Moving servo only if there is agreement about current direction across n previous readings

trajectorial-agreement.ino

const int SENSOR_PIN = 0;
const int CYCLE_DURATION = 250;

int average = 0;

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(9600);
}

int currentTrajectory = 0; // -1, 0, or 1
int lastReading = 0;
const int numTraj = 2;
int trajectories[numTraj] = {0, 0}; // last n trajectories

void loop() {
  int reading = analogRead(SENSOR_PIN);
  // average = (reading + (9 * average)) / 10;

  // rotate oldest reading out
  for (int i = 0; i < numTraj - 1; i++) {
    trajectories[i] = trajectories[i + 1];
  }

  // store new trajectory
  if (reading > lastReading) trajectories[numTraj - 1] = 1;
  else if (reading == lastReading) trajectories[numTraj - 1] = 0;
  else trajectories[numTraj - 1] = -1;

  // check to see if we have trajectorial agreement
  bool agreement = true;
  for (int i = 1; i < numTraj; i++) {
    agreement = agreement && (trajectories[i - 1] == trajectories[i]);
  }

  // if there's agreement, we change trajectory
  if (agreement) currentTrajectory = trajectories[0];

  // move in the direction of the consensus trajectory
  Serial.print("trajectories=[");
  for (int i = 0; i < numTraj; i++) {
    Serial.print(trajectories[i]);
    if(i < numTraj - 1) Serial.print(",");
  }
  Serial.print("], currentTrajectory=");
  Serial.println(currentTrajectory);

  // store lastReading
  lastReading = reading;

  int duration = map(reading, 0, 1023, 0, CYCLE_DURATION);
  Serial.print("reading=");
  Serial.print(reading);
  Serial.print(" duration=");
  Serial.print(duration);
  Serial.print(" average=");
  Serial.println(average);
  Serial.println();
  digitalWrite(LED_BUILTIN, HIGH);
  delay(duration);
  digitalWrite(LED_BUILTIN, LOW);
  delay(CYCLE_DURATION - duration);
}