/********* 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: "PID Positioning"
	- Source Code NOT compiled for: Arduino Uno
	- Source Code created on: 2024-06-24 14:34:47

********* 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/ErickSimoes/Ultrasonic
#include        //https://github.com/arduino-libraries/Servo

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

/***** DEFINITION OF DIGITAL INPUT PINS *****/
const uint8_t Ultrasonic_Sensor_HC_SR04_Echo_PIN_D3 = 3;

/***** DEFINITION OF DIGITAL OUTPUT PINS *****/
const uint8_t Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D2 = 2;

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t servo_Servomotor_PWMSignal_PIN_D5 = 5;

/***** DEFINITION OF OUTPUT RAW VARIABLES *****/
/***** used to store raw data *****/
bool Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D2_rawData = 0;
uint8_t servo_Servomotor_PWMSignal_PIN_D5_rawData = 0;

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

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
// Initialize the Ultrasonic sensor object with trigger and echo pins
Ultrasonic ultrasonic(Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D2, Ultrasonic_Sensor_HC_SR04_Echo_PIN_D3);

// Initialize the Servo object
Servo myservo;

/***** PID CONTROL VARIABLES *****/
float setPoint = 32.0; // Desired position in cm
float input, output;
float Kp = 2.0, Ki = 5.0, Kd = 1.0; // PID constants
float previousError = 0, integral = 0;
unsigned long lastTime;

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

    pinMode(Ultrasonic_Sensor_HC_SR04_Echo_PIN_D3, INPUT);
    pinMode(Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D2, OUTPUT);
    pinMode(servo_Servomotor_PWMSignal_PIN_D5, OUTPUT);

    // Attach the servo to the PWM pin
    myservo.attach(servo_Servomotor_PWMSignal_PIN_D5);

    lastTime = millis();
}

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

    // Read distance from the ultrasonic sensor
    input = ultrasonic.read();
    Serial.print("Distance in CM: ");
    Serial.println(input);

    // Compute PID control
    computePID();

    // Map the PID output to a servo position (0 to 180 degrees)
    int servoPosition = map(output, 0, 100, 0, 180); // Assuming max distance is 100 cm
    myservo.write(servoPosition); // Set the servo position

    delay(100); // Delay for 100 milliseconds
}

void updateOutputs()
{
    digitalWrite(Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D2, Ultrasonic_Sensor_HC_SR04_Trigger_PIN_D2_rawData);
    analogWrite(servo_Servomotor_PWMSignal_PIN_D5, servo_Servomotor_PWMSignal_PIN_D5_rawData);
}

void computePID()
{
    unsigned long now = millis();
    double timeChange = (double)(now - lastTime);

    // Calculate error
    float error = setPoint - input;

    // Integral term
    integral += (error * timeChange);

    // Derivative term
    float derivative = (error - previousError) / timeChange;

    // PID output
    output = Kp * error + Ki * integral + Kd * derivative;

    // Remember variables for next loop
    previousError = error;
    lastTime = now;
}

/* END CODE */