// Why does this not work ?
// I made this for the forum: https://forum.arduino.cc/t/unable-to-move-third-servo-on-nano-esp32/1165322
// But did not tell on the forum about it.
//
// 2025:
//   The solution was given in the Discord channel of Wokwi.
//   There are only two timers available!
//   The solution is to use another library.
//   * https://github.com/Dlloydev/ESP32-ESP32S2-AnalogWrite
//   * https://github.com/khoih-prog/ESP32_New_ISR_Servo
//   The library by Dlloydev can use up to 16 servo motors.
//
//   Note that the Arduino Uno/Mega/Nano use a single timer
//   with a singel interrupt, and create all the servo signals 
//   for many servo motors from that single interrupt.
//

#include <ESP32Servo.h>

Servo servoPitch;
Servo servoRoll;
Servo servoYaw;

int pitchPos;
int pitchDir = +2;
int rollPos;
int rollDir = +3;
int yawPos;
int yawDir = +4;

void setup() 
{
  Serial.begin(9600);
  Serial.println("------------------------------");
  Serial.println("    The sketch has started    ");
  Serial.println("------------------------------");

  servoPitch.attach(5);
  servoRoll.attach(6);
  servoYaw.attach(7);
} 

void loop() 
{
  pitchPos += pitchDir;
  rollPos += rollDir;
  yawPos += yawDir;

  if(pitchPos < 0 or pitchPos > 180)
  {
    pitchDir = -pitchDir;
    pitchPos = constrain(pitchPos, 0, 180);
  }

  if(rollPos < 0 or rollPos > 180)
  {
    rollDir = -rollDir;
    rollPos = constrain(rollPos, 0, 180);
  }

  if(yawPos < 0 or yawPos > 180)
  {
    yawDir = -yawDir;
    yawPos = constrain(yawPos, 0, 180);
  }

  servoPitch.write(pitchPos);
  servoRoll.write(rollPos);
  servoYaw.write(yawPos);

  delay(20);
}