#include <ESP32Servo.h>

#define IN_X 32
#define IN_Y 33
#define OUT_X 12
#define OUT_Y 13
#define size 5
#define middle size/2

Servo servoX, servoY;

int valX[size] = {0, 0, 0, 0, 0};
int valY[size] = {0, 0, 0, 0, 0};

int cmp(const void * a, const void * b){
  return( (int)a - (int)b );
}
void filter(){
  static byte i = 0;
  valX[i] = analogRead(IN_X);
  valY[i] = analogRead(IN_Y);

  qsort(valX, size, sizeof(int), cmp);
  qsort(valY, size, sizeof(int), cmp);

  if(++i % size == 0) i = 0;
}
void control(){
  int posX, posY;
  posX = valX[middle];
  posY = valY[middle];
  printf("posX:%d\n", posX);
  printf("posY:%d\n", posY);
  servoX.write(posX);
  servoY.write(posY);
  delay(15);
}
void setup(){
  analogSetAttenuation(ADC_11db);
  analogSetWidth(10);
  servoX.attach(OUT_X, 500, 2400);
  servoY.attach(OUT_Y, 500, 2400);
}
void loop(){
  filter();
  control();
}