#include "L298N_ESP32.h"
#include "Servo_ESP32.h"

// Motor pins
#define MOTOR1_PIN1     27
#define MOTOR1_PIN2     26
#define MOTOR1_ENABLE   14
#define MOTOR2_PIN1     25
#define MOTOR2_PIN2     33
#define MOTOR2_ENABLE   32

// Servo pins
#define SERVO1_PIN      13
#define SERVO2_PIN      12

// Create motor objects
L298N_ESP32 motor1(MOTOR1_PIN1, MOTOR1_PIN2, MOTOR1_ENABLE, 0);
L298N_ESP32 motor2(MOTOR2_PIN1, MOTOR2_PIN2, MOTOR2_ENABLE, 1);

// Create servo objects (using PWM channels 2 and 3)
Servo_ESP32 servo1(SERVO1_PIN, 2);
Servo_ESP32 servo2(SERVO2_PIN, 3);

void setup() {
  Serial.begin(115200);
  
  // Initialize motors
  motor1.begin();
  motor2.begin();
  motor1.setMin(50); // Set minimum speed for motor1
  motor1.setMax(200); // Set maximum speed for motor1
  motor2.setMin(50); // Set minimum speed for motor2
  motor2.setMax(200); // Set maximum speed for motor2
  
  // Initialize servos
  servo1.begin();
  servo2.begin();
  
  servo1.setMin(10); // Set minimum position for servo1
  servo1.setMax(170); // Set maximum position for servo1
  servo2.setMin(20); // Set minimum position for servo2
  servo2.setMax(160); // Set maximum position for servo2
  
  Serial.println("Motors and servos initialized");
}

void loop() {
  // Move motors forward and sweep servos
  Serial.println("Moving motors forward and sweeping servos...");
  
  motor1.forward();
  motor2.forward();
  
  motor1.setPercentage(75); // Set motor1 speed to 75%
  motor2.setPercentage(75); // Set motor2 speed to 75%
  
  servo1.sweep(10, 170, 15); // Sweep servo1 from position 10 to position 170 with a delay of 15ms between steps
  servo2.sweep(20, 160, 15); // Sweep servo2 from position 20 to position 160 with a delay of 15ms between steps
  
  delay(2000);

  // Stop motors and center servos
  Serial.println("Stopping motors and centering servos...");
  
  motor1.stop();
  motor2.stop();
  
  servo1.goInit(); // Move servo1 to its initial (center) position
  servo2.goInit(); // Move servo2 to its initial (center) position
  
  delay(1000);

  // Move motors backward and control servos using percentages
  Serial.println("Moving motors backward and controlling servos by percentage...");
  
  motor1.backward();
  motor2.backward();
  
  motor1.goMin(); // Run motor1 at its minimum speed
  motor2.goMin(); // Run motor2 at its minimum speed
  
  for (int i = 0; i <= 100; i += 10) {
    servo1.percent(i);         // Move servo1 to a percentage of its range (0% to max)
    servo2.percent(100 - i);   // Move servo2 to the opposite percentage of its range (max to min)
    delay(200);
  }
  
  delay(2000);

  // Stop all motors and reset servos to center positions
  Serial.println("Stopping all motors and resetting servos...");
  
  motor1.stop();
  motor2.stop();
  
  servo1.goInit(); // Reset servo1 to center position
  servo2.goInit(); // Reset servo2 to center position
  
  delay(1000);

  
}