/*
  Based on Koepels ESP32-S3 example
*/

#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(25);
  servoRoll.attach(26);
  servoYaw.attach(27);
}

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);
}