/********* 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: "Sensory Servo"
	- Source Code compiled for: Arduino Mega
	- Source Code created on: 2024-02-22 20:45:17

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* via the first sensor distance readings less than */
	/* 20 cm move the servo to the angle 10 if no */
	/* readings at 20 cm move the servo to 90 angle and */
	/* if the second sensor reads an object less than 20 */
	/* cm far move the servo to the angle 170 */
/****** END SYSTEM REQUIREMENTS *****/

/****** DEFINITION OF LIBRARIES *****/
#include <Ultrasonic.h> //https://github.com/ErickSimoes/Ultrasonic
#include <Servo.h>     //https://github.com/arduino-libraries/Servo

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
/* via the first sensor distance readings less than */
/* 20 cm move the servo to the angle 10 if no */
/* readings at 20 cm move the servo to 90 angle and */
/* if the second sensor reads an object less than 20 */
/* cm far move the servo to the angle 170 */
/****** END SYSTEM REQUIREMENTS *****/

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

/***** DEFINITION OF DIGITAL INPUT PINS *****/
const uint8_t s1_HC_SR04_Echo_PIN_D3 = 3;
const uint8_t s2_HC_SR04_Echo_PIN_D6 = 6;

/***** DEFINITION OF DIGITAL OUTPUT PINS *****/
const uint8_t s1_HC_SR04_Trigger_PIN_D2 = 2;
const uint8_t s2_HC_SR04_Trigger_PIN_D5 = 5;

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t servo_Servomotor_PWMSignal_PIN_D4 = 4;

/***** DEFINITION OF OUTPUT RAW VARIABLES *****/
/***** used to store raw data *****/
bool s1_HC_SR04_Trigger_PIN_D2_rawData = 0;
bool s2_HC_SR04_Trigger_PIN_D5_rawData = 0;
uint8_t servo_Servomotor_PWMSignal_PIN_D4_rawData = 0;

/***** DEFINITION OF OUTPUT PHYSICAL VARIABLES *****/
/***** used to store data after characteristic curve transformation *****/
float s1_HC_SR04_Trigger_PIN_D2_phyData = 0.0;
float s2_HC_SR04_Trigger_PIN_D5_phyData = 0.0;
float servo_Servomotor_PWMSignal_PIN_D4_phyData = 0.0;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
Ultrasonic ultrasonic1(s1_HC_SR04_Trigger_PIN_D2, s1_HC_SR04_Echo_PIN_D3);
Ultrasonic ultrasonic2(s2_HC_SR04_Trigger_PIN_D5, s2_HC_SR04_Echo_PIN_D6);
Servo servo;

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

  pinMode(s1_HC_SR04_Echo_PIN_D3, INPUT);
  pinMode(s2_HC_SR04_Echo_PIN_D6, INPUT);

  pinMode(s1_HC_SR04_Trigger_PIN_D2, OUTPUT);
  pinMode(s2_HC_SR04_Trigger_PIN_D5, OUTPUT);
  pinMode(servo_Servomotor_PWMSignal_PIN_D4, OUTPUT);

  servo.attach(servo_Servomotor_PWMSignal_PIN_D4);
}

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

  // Read distances from ultrasonic sensors
  unsigned int distance1 = ultrasonic1.read();
  unsigned int distance2 = ultrasonic2.read();

  // System Requirement 1: Move the servo based on distance readings
  if (distance1 < 20)
  {
    servo.write(10); // Angle 10
  }
  else if (distance1 >= 20 && distance1 < 40)
  {
    servo.write(90); // Angle 90
  }

  if (distance2 < 20)
  {
    servo.write(170); // Angle 170
  }

  delay(100); // Add delay if needed
}

void updateOutputs()
{
  digitalWrite(s1_HC_SR04_Trigger_PIN_D2, s1_HC_SR04_Trigger_PIN_D2_rawData);
  digitalWrite(s2_HC_SR04_Trigger_PIN_D5, s2_HC_SR04_Trigger_PIN_D5_rawData);
  servo.write(servo_Servomotor_PWMSignal_PIN_D4_rawData);
}