/********* 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: Motor Control
	- Source Code NOT compiled for: Arduino Uno
	- Source Code created on: 2025-10-29 17:54:21

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* Implement a servo motor control system that */
	/* adjusts its angle based on input from connected */
	/* sensors, improving automation precision in the */
	/* project. */
/****** END SYSTEM REQUIREMENTS *****/


/* START CODE */

// System Requirements: Implement a servo motor control system that adjusts its angle based on input from connected sensors, improving automation precision in the project.

#include <Servo.h>
#include <NewPing.h>

#define SERVO_PIN 3
#define ULTRASONIC_SENSOR_TRIG 11
#define ULTRASONIC_SENSOR_ECHO 12
#define MAX_REGULAR_MOTOR_SPEED 80
#define MAX_MOTOR_ADJUST_SPEED 170
#define DISTANCE_TO_CHECK 40

//Right motor
int enableRightMotor=5;
int rightMotorPin1=7;
int rightMotorPin2=8;

//Left motor
int enableLeftMotor=6;
int leftMotorPin1=9;
int leftMotorPin2=10;

NewPing mySensor(ULTRASONIC_SENSOR_TRIG, ULTRASONIC_SENSOR_ECHO, 400);
Servo myServo;

// Function to rotate motors at specified speeds
void rotateMotor(int rightMotorSpeed, int leftMotorSpeed) {
  if (rightMotorSpeed < 0) {
    digitalWrite(rightMotorPin1,LOW);
    digitalWrite(rightMotorPin2,HIGH);    
  } else if (rightMotorSpeed >= 0) {
    digitalWrite(rightMotorPin1,HIGH);
    digitalWrite(rightMotorPin2,LOW);      
  }
  if (leftMotorSpeed < 0) {
    digitalWrite(leftMotorPin1,LOW);
    digitalWrite(leftMotorPin2,HIGH);    
  } else if (leftMotorSpeed >= 0) {
    digitalWrite(leftMotorPin1,HIGH);
    digitalWrite(leftMotorPin2,LOW);      
  }
  analogWrite(enableRightMotor, abs(rightMotorSpeed));
  analogWrite(enableLeftMotor, abs(leftMotorSpeed));    
}

void setup() {
  // Initialize motor control pins
  pinMode(enableRightMotor,OUTPUT);
  pinMode(rightMotorPin1,OUTPUT);
  pinMode(rightMotorPin2,OUTPUT);

  pinMode(enableLeftMotor,OUTPUT);
  pinMode(leftMotorPin1,OUTPUT);
  pinMode(leftMotorPin2,OUTPUT);

  // Attach servo
  myServo.attach(SERVO_PIN);
  myServo.write(90); // Set servo to center position
}

void loop() {
  // Read distance using ultrasonic sensor
  int distance = mySensor.ping_cm();

  // If object detected within threshold, perform avoidance maneuver
  if (distance > 0 && distance < DISTANCE_TO_CHECK) {
    // Stop motors
    rotateMotor(0, 0);
    delay(500);
    
    // Reverse motors
    rotateMotor(-MAX_MOTOR_ADJUST_SPEED, -MAX_MOTOR_ADJUST_SPEED);
    delay(200);
    
    // Stop motors
    rotateMotor(0, 0);
    delay(500);
    
    // Rotate servo to left
    myServo.write(180);
    delay(500);
    
    // Read left side distance
    int distanceLeft = mySensor.ping_cm();

    // Rotate servo to right
    myServo.write(0);
    delay(500);

    // Read right side distance
    int distanceRight = mySensor.ping_cm();

    // Return servo to center
    myServo.write(90);
    delay(500);

    // Decide turn direction based on distances
    if (distanceLeft == 0) {
      rotateMotor(MAX_MOTOR_ADJUST_SPEED, -MAX_MOTOR_ADJUST_SPEED);
      delay(550);
    } else if (distanceRight == 0) {
      rotateMotor(-MAX_MOTOR_ADJUST_SPEED, MAX_MOTOR_ADJUST_SPEED);
      delay(550);
    } else if (distanceLeft >= distanceRight) {
      rotateMotor(MAX_MOTOR_ADJUST_SPEED, -MAX_MOTOR_ADJUST_SPEED);
      delay(550);
    } else {
      rotateMotor(-MAX_MOTOR_ADJUST_SPEED, MAX_MOTOR_ADJUST_SPEED);
      delay(550);
    }
    // Stop motors after maneuver
    rotateMotor(0, 0);
    delay(550);
  } else {
    // Move forward
    rotateMotor(MAX_REGULAR_MOTOR_SPEED, MAX_REGULAR_MOTOR_SPEED);
  }
}

/* END CODE */