/********* 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: Object_Tracking_Robot
	- Source Code compiled for: Arduino Uno
	- Source Code created on: 2024-01-07 21:01:40

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* start my moving myServo to 180 then back to 0 then */
	/* leave at 90 degrees.  go to main loop. */
/****** SYSTEM REQUIREMENT 2 *****/
	/* if distance between 5 and 15, move forward. if */
	/* rightSensor is high and leftSensor is low, turn */
	/* right. if rightSensor is low and leftSensor is */
	/* high, turn left. if distance is greater than 15, */
	/* stop. */
/****** END SYSTEM REQUIREMENTS *****/

/****** DEFINITION OF LIBRARIES *****/
#include <Arduino.h>
#include <Ultrasonic.h>
#include <Servo.h>

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);

/***** DEFINITION OF DIGITAL INPUT PINS *****/
const uint8_t ultraSonic_HC_SR04_Echo_PIN = 3;
const uint8_t rightSensor_PIN = 4;
const uint8_t leftSensor_PIN = 5;

/***** DEFINITION OF DIGITAL OUTPUT PINS *****/
const uint8_t ultraSonic_HC_SR04_Trigger_PIN = 2;
const uint8_t motor_L293xQuadrupleHalf_HDriver_1A_PIN = 7;
const uint8_t motor_L293xQuadrupleHalf_HDriver_2A_PIN = 8;
const uint8_t motor_L293xQuadrupleHalf_HDriver_3A_PIN = 11;
const uint8_t motor_L293xQuadrupleHalf_HDriver_4A_PIN = 12;

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t myServo_Servomotor_PWMSignal_PIN = 6;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
Ultrasonic ultrasonic(ultraSonic_HC_SR04_Trigger_PIN, ultraSonic_HC_SR04_Echo_PIN);
Servo myServo;

void setup(void)
{
  // put your setup code here, to run once:

  pinMode(ultraSonic_HC_SR04_Echo_PIN, INPUT);
  pinMode(rightSensor_PIN, INPUT);
  pinMode(leftSensor_PIN, INPUT);

  pinMode(ultraSonic_HC_SR04_Trigger_PIN, OUTPUT);
  pinMode(motor_L293xQuadrupleHalf_HDriver_1A_PIN, OUTPUT);
  pinMode(motor_L293xQuadrupleHalf_HDriver_2A_PIN, OUTPUT);
  pinMode(motor_L293xQuadrupleHalf_HDriver_3A_PIN, OUTPUT);
  pinMode(motor_L293xQuadrupleHalf_HDriver_4A_PIN, OUTPUT);
  pinMode(myServo_Servomotor_PWMSignal_PIN, OUTPUT);

  myServo.attach(myServo_Servomotor_PWMSignal_PIN);

  // SYSTEM REQUIREMENT 1: move myServo to 180 degrees, then back to 0 degrees, and leave at 90 degrees
  myServo.write(180);
  delay(1000);
  myServo.write(0);
  delay(1000);
  myServo.write(90);
}

void loop(void)
{
  // put your main code here, to run repeatedly:

  // SYSTEM REQUIREMENT 2: logic for moving forward, turning right, turning left, and stopping based on distance and sensor readings

  // Read the distance from the ultrasonic sensor
  unsigned int distance = ultrasonic.read();

  // Read the sensor values
  bool rightSensor = digitalRead(rightSensor_PIN);
  bool leftSensor = digitalRead(leftSensor_PIN);

  // Check if the distance is between 5 and 15
  if (distance >= 5 && distance <= 15)
  {
    // Move forward
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_1A_PIN, HIGH);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_2A_PIN, LOW);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_3A_PIN, HIGH);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_4A_PIN, LOW);
  }
  else if (rightSensor && !leftSensor)
  {
    // Turn right
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_1A_PIN, LOW);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_2A_PIN, HIGH);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_3A_PIN, HIGH);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_4A_PIN, LOW);
  }
  else if (!rightSensor && leftSensor)
  {
    // Turn left
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_1A_PIN, HIGH);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_2A_PIN, LOW);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_3A_PIN, LOW);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_4A_PIN, HIGH);
  }
  else
  {
    // Stop
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_1A_PIN, LOW);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_2A_PIN, LOW);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_3A_PIN, LOW);
    digitalWrite(motor_L293xQuadrupleHalf_HDriver_4A_PIN, LOW);
  }
}