Dual Servos rev_02

/********* Pleasedontcode.com **********

    Pleasedontcode thanks you for automatic code generation! Enjoy your code!

    - Terms and Conditions:
    You have a non-exclusive, revocable, worldwide, royalty-free license
    for personal and commercial use. Attribution is optional; modifications
    are allowed, but you're responsible for code maintenance. We're not
    liable for any loss or damage. For full terms,
    please visit pleasedontcode.com/termsandconditions.

    - Project: Dual Servos
    - Source Code compiled for: ESP32 DevKit V1
    - Source Code created on: 2025-09-24 12:55:14

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* According to the code make the code perfect for */
    /* the project */
/****** END SYSTEM REQUIREMENTS *****/




/****** DEFINITION OF LIBRARIES *****/
#include <ESP32Servo.h>

/****** PROTOTYPES *****/
void setup(void);
void loop(void);
int mapJoystick(int value, bool x_axis);

/***** DEFINITION OF PWM OUTPUT PINS *****/
// Updated to reflect actual servo pins used in USER CODE
const uint8_t SERVO1_PIN = 16;
const uint8_t SERVO2_PIN = 17;

// Note: The original PRELIMINARY code defined D4 and D13 as PWM pins.
// Those are not used with the current servo library configuration.
// If you want to retain them, you can define and rewire, but ensure compatibility with ESP32Servo.


/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
Servo servo1, servo2;

/****** DEFINITION OF JOYSTICK AND CONTROL PINS *****/
const int JOY1_X_PIN = 34;
const int JOY1_Y_PIN = 35;
const int JOY1_BTN_PIN = 32;
const int JOY2_X_PIN = 36;
const int JOY2_Y_PIN = 39;
const int JOY2_BTN_PIN = 33;

// Calibration values for analog read (tune as needed)
const int ANALOG_MIN = 0;      // ESP32 ADC minimum
const int ANALOG_MAX = 4095;   // ESP32 ADC maximum
const int CENTER = (ANALOG_MAX + ANALOG_MIN) / 2;
const int DEADZONE = 100;      // ignore small movement near center

int angle1 = 0, angle2 = 0;
unsigned long lastUpdate1 = 0, lastUpdate2 = 0;
const unsigned long updateInterval = 10; // ms between updates
unsigned long debounceTime1 = 0, debounceTime2 = 0;
const unsigned long debounceDelay = 150; // ms


void setup(void)
{
  // Initialize PWM timers for ESP32Servo
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  servo1.setPeriodHertz(50);
  servo2.setPeriodHertz(50);
  // Attach servos to pins with typical 500-2500 us pulse range
  servo1.attach(SERVO1_PIN, 500, 2500);
  servo2.attach(SERVO2_PIN, 500, 2500);

  // Joystick button inputs
  pinMode(JOY1_BTN_PIN, INPUT_PULLUP);
  pinMode(JOY2_BTN_PIN, INPUT_PULLUP);

  // Start serial for debugging
  Serial.begin(115200);
  // Center the servos initially
  angle1 = 90; angle2 = 90;
  servo1.write(angle1);
  servo2.write(angle2);
}

int mapJoystick(int value, bool x_axis) {
  // Map a single axis value to 0-180 range with a deadzone around center
  if (abs(value - CENTER) < DEADZONE) return -1; // within dead zone
  int angle = 0;
  if (value < CENTER) {
    // From min to center (left/up)
    angle = map(value, ANALOG_MIN, CENTER - DEADZONE, 0, 90);
  } else {
    // From center to max (right/down)
    angle = map(value, CENTER + DEADZONE, ANALOG_MAX, 90, 180);
  }
  return constrain(angle, 0, 180);
}

void loop() {
  unsigned long now = millis();

  // Joystick 1 and Servo 1
  int joy1_x = analogRead(JOY1_X_PIN);
  int joy1_y = analogRead(JOY1_Y_PIN);
  bool joy1_btn = (digitalRead(JOY1_BTN_PIN) == LOW);

  if (now - lastUpdate1 > updateInterval) {
    int dx = abs(joy1_x - CENTER);
    int dy = abs(joy1_y - CENTER);
    int mapped_angle = -1;
    // Prioritize axis with greater deviation from center
    if (dx > dy) {
      mapped_angle = mapJoystick(joy1_x, true);
    } else {
      mapped_angle = mapJoystick(joy1_y, false);
    }
    if (mapped_angle != -1) {
      angle1 = mapped_angle;
    }
    // Button to step servo by +1 degree when pressed (simple example)
    if (joy1_btn && (now - debounceTime1 > debounceDelay)) {
      angle1 = constrain(angle1 + 1, 0, 180);
      debounceTime1 = now;
    }
    servo1.write(angle1);
    lastUpdate1 = now;
  }

  // Joystick 2 and Servo 2
  int joy2_x = analogRead(JOY2_X_PIN);
  int joy2_y = analogRead(JOY2_Y_PIN);
  bool joy2_btn = (digitalRead(JOY2_BTN_PIN) == LOW);

  if (now - lastUpdate2 > updateInterval) {
    int dx2 = abs(joy2_x - CENTER);
    int dy2 = abs(joy2_y - CENTER);
    int mapped_angle2 = -1;
    if (dx2 > dy2) {
      mapped_angle2 = mapJoystick(joy2_x, true);
    } else {
      mapped_angle2 = mapJoystick(joy2_y, false);
    }
    if (mapped_angle2 != -1) {
      angle2 = mapped_angle2;
    }
    if (joy2_btn && (now - debounceTime2 > debounceDelay)) {
      angle2 = constrain(angle2 + 1, 0, 180);
      debounceTime2 = now;
    }
    servo2.write(angle2);
    lastUpdate2 = now;
  }
}

/* END CODE */