/*
 * Continuous Rotation Servo Control for ESP32
 * This sketch controls two continuous rotation servos for variable speed and direction.
 */

#include <ESP32Servo.h>  // Include the ESP32Servo library

Servo myservoLeft;   // Create servo object to control the left servo
Servo myservoRight;  // Create servo object to control the right servo

const int leftServoPin = 5;   // GPIO pin for the left servo
const int rightServoPin = 18; // GPIO pin for the right servo
int speed = 0;                // Variable to store the servo speed

void setup() {
  myservoLeft.attach(leftServoPin);   // Attach the left servo to the specified pin
  myservoRight.attach(rightServoPin); // Attach the right servo to the specified pin
  Serial.begin(9600);                 // Initialize serial communication for debugging
  Serial.println("Continuous rotation servo control initialized");
}

void loop() {
  // Forward motion: Increase speed from neutral (90) to max (180)
  for (speed = 90; speed < 180; speed++) {
    myservoLeft.write(speed);          // Rotate left servo forward
    myservoRight.write(180 - speed);   // Rotate right servo forward in the opposite direction
    delay(20);                         // Delay for smooth motion
    Serial.print("Speed: ");
    Serial.println(speed);
  }

  // Backward motion: Decrease speed from max (180) to neutral (90)
  for (speed = 180; speed >= 90; speed--) {
    myservoLeft.write(speed);          // Rotate left servo backward
    myservoRight.write(180 - speed);   // Rotate right servo backward in the opposite direction
    delay(20);                         // Delay for smooth motion
    Serial.print("Speed: ");
    Serial.println(speed);
  }
}