#include <ESP32Servo.h>

 

const int leftServoPin = 6;

const int rightServoPin = 5;

 

const int leftServoNeutralPoint = 1480;

const int rightServoNeutralPoint = 1500; // Assuming this is correct

 

Servo servoLW;

Servo servoRW;

 

// Function to convert a speed (-100 to 100) to a PWM value for each servo

int convertToLeftPWM(int speed) {

  // Map speed from [-100, 100] to a PWM range, e.g., [1200, 1760]

  return map(speed, -100, 100, 1760, 1200); // Note the inverted mapping for the left servo

}

 

int convertToRightPWM(int speed) {

  // Map speed from [-100, 100] to a PWM range, e.g., [1240, 1760]

  return map(speed, -100, 100, 1240, 1760);

}

 

// Function to control an arc turn

void arcTurn(int baseSpeed, float turnFactor, bool turnRight) {

  int leftSpeed;

  int rightSpeed;

 

  if (turnRight) {

    leftSpeed = baseSpeed * (1 + turnFactor);

    rightSpeed = baseSpeed;

  } else {

    leftSpeed = baseSpeed;

    rightSpeed = baseSpeed * (1 + turnFactor);

  }

 

  leftSpeed = constrain(leftSpeed, -100, 100);

  rightSpeed = constrain(rightSpeed, -100, 100);

 

  servoLW.writeMicroseconds(convertToLeftPWM(leftSpeed));

  servoRW.writeMicroseconds(convertToRightPWM(rightSpeed));

}

 

void setup() {

  Serial.begin(115200);

  servoLW.attach(leftServoPin);

  servoRW.attach(rightServoPin);

}

 

void loop() {

  Serial.println("Performing a right arc turn...");

  arcTurn(50, 1.5, true); // Increased turnFactor

  delay(3000);

 

  Serial.println("Stopping robot...");

  arcTurn(0, 0, true);

  delay(2000);

 

  Serial.println("Performing a left arc turn...");

  arcTurn(50, 1.5, false); // Increased turnFactor

  delay(3000);

 

  Serial.println("Stopping robot...");

  arcTurn(0, 0, false);

  delay(2000);

}