/********* 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: "Conditional Stopping"
	- Source Code NOT compiled for: ESP32 DevKit V1
	- Source Code created on: 2024-06-04 09:27:23

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* move wiper motor using esp32 and motor driver */
	/* bts7960, and when ultrasonic sensor detect */
	/* something, turn the motor off */
/****** END SYSTEM REQUIREMENTS *****/

/****** DEFINITION OF LIBRARIES *****/
#include <Ultrasonic.h>    //https://github.com/ErickSimoes/Ultrasonic
#include <BTS7960.h>       //https://github.com/luisllamasbinaburo/Arduino-BTS7960

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

/***** DEFINITION OF DIGITAL INPUT PINS *****/
const uint8_t sensor_ultrasonik_HC_SR04_Echo_PIN_D13 = 13;
const uint8_t sensor_ultrasonik_HC_SR04_Echo_PIN_D16 = 16;

/***** DEFINITION OF DIGITAL OUTPUT PINS *****/
const uint8_t sensor_ultrasonik_HC_SR04_Trigger_PIN_D4 = 4;
const uint8_t sensor_ultrasonik_HC_SR04_Trigger_PIN_D14 = 14;

/***** DEFINITION OF MOTOR CONTROL PINS *****/
const uint8_t motor_EN_PIN = 8;
const uint8_t motor_L_PWM_PIN = 9;
const uint8_t motor_R_PWM_PIN = 10;

/***** DEFINITION OF OUTPUT RAW VARIABLES *****/
/***** used to store raw data *****/
bool sensor_ultrasonik_HC_SR04_Trigger_PIN_D4_rawData = 0;
bool sensor_ultrasonik_HC_SR04_Trigger_PIN_D14_rawData = 0;

/***** DEFINITION OF OUTPUT PHYSICAL VARIABLES *****/
/***** used to store data after characteristic curve transformation *****/
float sensor_ultrasonik_HC_SR04_Trigger_PIN_D4_phyData = 0.0;
float sensor_ultrasonik_HC_SR04_Trigger_PIN_D14_phyData = 0.0;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
Ultrasonic ultrasonic1(sensor_ultrasonik_HC_SR04_Trigger_PIN_D4, sensor_ultrasonik_HC_SR04_Echo_PIN_D13); // Initialize Ultrasonic object for first sensor
Ultrasonic ultrasonic2(sensor_ultrasonik_HC_SR04_Trigger_PIN_D14, sensor_ultrasonik_HC_SR04_Echo_PIN_D16); // Initialize Ultrasonic object for second sensor

// Initialize BTS7960 motor controller object
BTS7960 motorController(motor_EN_PIN, motor_L_PWM_PIN, motor_R_PWM_PIN);

void setup(void)
{
    // put your setup code here, to run once:
    Serial.begin(9600); // Initialize serial communication

    pinMode(sensor_ultrasonik_HC_SR04_Echo_PIN_D13, INPUT);
    pinMode(sensor_ultrasonik_HC_SR04_Echo_PIN_D16, INPUT);

    pinMode(sensor_ultrasonik_HC_SR04_Trigger_PIN_D4, OUTPUT);
    pinMode(sensor_ultrasonik_HC_SR04_Trigger_PIN_D14, OUTPUT);

    // Enable the motor controller
    motorController.Enable();
}

void loop(void)
{
    // put your main code here, to run repeatedly:
    updateOutputs(); // Refresh output data

    // Read distance from the first ultrasonic sensor
    int distance1 = ultrasonic1.read();
    Serial.print("Distance from sensor 1 in CM: ");
    Serial.println(distance1);

    // Read distance from the second ultrasonic sensor
    int distance2 = ultrasonic2.read();
    Serial.print("Distance from sensor 2 in CM: ");
    Serial.println(distance2);

    // Control the motor based on the distance from the ultrasonic sensors
    if (distance1 < 20 || distance2 < 20) {
        // If the distance from any sensor is less than 20 cm, stop the motor
        motorController.Stop();
    } else {
        // Otherwise, turn the motor to the left
        motorController.TurnLeft(100); // Example speed value
    }

    delay(1000); // Wait for 1 second before the next reading
}

void updateOutputs()
{
    digitalWrite(sensor_ultrasonik_HC_SR04_Trigger_PIN_D4, sensor_ultrasonik_HC_SR04_Trigger_PIN_D4_rawData);
    digitalWrite(sensor_ultrasonik_HC_SR04_Trigger_PIN_D14, sensor_ultrasonik_HC_SR04_Trigger_PIN_D14_rawData);
}

/* END CODE */