/*
  Forum:https://forum.arduino.cc/t/joystick-upgrade-for-rc-car/1335057/22
  Wokwi: https://wokwi.com/projects/418529559929422849

  2024/12/28
  ec2021

*/

#include <ESP32Servo.h>


// Some constants to define the center, a delta and the minimum and maximum angle of the Servo
// to the left and the right side
const int centerAngle {90};
const int deltaAngle {45};
const int maxLeft {centerAngle - deltaAngle};
const int maxRight {centerAngle + deltaAngle};

// These values define the range of the joystick axis (relaized in Wokwi by a slide potentiometer)
// With a real joystick the values have to be replaced by the maximum und minimum as measured
const uint16_t minValue {0};
const uint16_t maxValue {4095};

// Constants for the pins used
const byte axis1Pin {25};
const byte servoPin {14};

// Global declarations
Servo myMotor;
unsigned long lastControl = 0;


void setup() {
  pinMode(axis1Pin, INPUT);
  Serial.begin(115200);
  myMotor.attach(servoPin);
  // Set the servo to centerAngle
  myMotor.write(centerAngle);
}

void loop() {
  // This function is called every time loop is performed
  controlEveryMsec(100);
}

void controlEveryMsec(unsigned long interval) {
  // The content of the if-clause will only be performed
  // if more than "interval" msecs have expired
  // Usually it does not make sense to control the servo
  // e.g. too often as there is a human "in the loop" ...
  if (millis() - lastControl > interval) {
    lastControl = millis();
    // The next function returns averaged data to reduce
    // noise
    uint16_t val = readJoystickAxis(axis1Pin);
    // The next function maps the result
    // to the defined angle range
    uint16_t angle = mappedAngle(val);
    // This result is written to the servo
    // and controls the angle of the Servo lever
    myMotor.write(angle);
  }
}

// This function reads the axis value noOfData times
// and returns the sum of all readings divided by noOfData
// There is a small delay in microSeconds so that the readings
// have some time to settle (not really necessary) ...
uint16_t readJoystickAxis(byte axisPin) {
  const uint16_t noOfData {20};
  unsigned long data = 0;
  for (int i = 0; i < noOfData; i++) {
    data += analogRead(axisPin);
    delayMicroseconds(1);
  }
  return data / noOfData;
}


// This function maps the axis data to the defined Servo angle
uint16_t mappedAngle(uint16_t value) {
  uint16_t angle = map(value, minValue, maxValue, maxLeft, maxRight);
  return angle;
}