/********* 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: Servo Control
	- Source Code NOT compiled for: Arduino Uno
	- Source Code created on: 2024-05-20 22:39:24

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* Implement Arduino PID control algorithm to */
	/* maintain ball position within 32 cm beam using */
	/* Ultrasonic sensor and 360-degree Servo motor. */
/****** END SYSTEM REQUIREMENTS *****/

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

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

/***** DEFINITION OF DIGITAL INPUT PINS *****/
const uint8_t Ultrasonic_Sensor_HC_SR04_Echo_PIN_D6 = 6;

/***** DEFINITION OF DIGITAL OUTPUT PINS *****/
const uint8_t Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D5 = 5;

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t myservo_Servomotor_PWMSignal_PIN_D3 = 3;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
Servo myservo; // Initialize Servo object
Ultrasonic ultrasonic(Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D5, Ultrasonic_Sensor_HC_SR04_Echo_PIN_D6); // Initialize Ultrasonic object

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

    pinMode(Ultrasonic_Sensor_HC_SR04_Echo_PIN_D6, INPUT);
    pinMode(Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D5, OUTPUT);
    pinMode(myservo_Servomotor_PWMSignal_PIN_D3, OUTPUT);

    myservo.attach(myservo_Servomotor_PWMSignal_PIN_D3); // Attach Servo object to PWM pin
}

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

    // Implement PID control algorithm to maintain ball position within 32 cm beam
    float distance = ultrasonic.read(); // Read distance from ultrasonic sensor
    float error = 32.0 - distance; // Calculate error
    float kp = 0.1; // Proportional gain
    int servoAngle = 90 + kp * error; // Calculate servo angle based on error

    // Ensure servo angle is within limits
    if (servoAngle < 0) {
        servoAngle = 0;
    } else if (servoAngle > 180) {
        servoAngle = 180;
    }

    myservo.write(servoAngle); // Set servo angle based on PID control

    delay(100); // Delay for stability
}

void updateOutputs(void)
{
    // No need to update outputs separately in this case
}

/* END CODE */