**Sensor Control** rev_01

/********* 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: **Sensor Control**
    - Source Code NOT compiled for: ESP8266 NodeMCU V1.0
    - Source Code created on: 2025-02-17 21:07:50

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

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* Control two servomotors using PWM signals while */
    /* reading data from an MPU6050 sensor for motion */
    /* detection. Ensure smooth operation and */
    /* responsiveness in real-time applications. */
/****** END SYSTEM REQUIREMENTS *****/

/* START CODE */

/****** DEFINITION OF LIBRARIES *****/
#include <Wire.h>
#include <MPU6050.h>    //https://github.com/electroniccats/mpu6050
#include <Deneyap_Servo.h>  //https://github.com/deneyapkart/deneyap-servo-arduino-library

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

/***** DEFINITION OF DIGITAL INPUT PINS *****/
const uint8_t sensor_MPU6050_Interrupt_PIN_D5       = 5;

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t servo1_Servomotor_PWMSignal_PIN_D6        = 6;
const uint8_t servo2_Servomotor_PWMSignal_PIN_D7        = 7;

/***** DEFINITION OF I2C PINS *****/
const uint8_t sensor_MPU6050_I2C_PIN_SDA_D2     = 2;
const uint8_t sensor_MPU6050_I2C_PIN_SCL_D1     = 1;
const uint8_t sensor_MPU6050_I2C_SLAVE_ADDRESS      = 104;

/***** DEFINITION OF OUTPUT RAW VARIABLES *****/
uint8_t servo1_Servomotor_PWMSignal_PIN_D6_rawData      = 0;
uint8_t servo2_Servomotor_PWMSignal_PIN_D7_rawData      = 0;

/***** DEFINITION OF OUTPUT PHYSICAL VARIABLES *****/
float   servo1_Servomotor_PWMSignal_PIN_D6_phyData      = 0.0;
float   servo2_Servomotor_PWMSignal_PIN_D7_phyData      = 0.0;

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
// Create instances for MPU6050 and Servos
MPU6050 mpu(sensor_MPU6050_I2C_SLAVE_ADDRESS);
Servo servo1;
Servo servo2;

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

    pinMode(sensor_MPU6050_Interrupt_PIN_D5, INPUT);
    pinMode(servo1_Servomotor_PWMSignal_PIN_D6, OUTPUT);
    pinMode(servo2_Servomotor_PWMSignal_PIN_D7, OUTPUT);

    // Setup for built-in LED
    pinMode(LED_BUILTIN, OUTPUT); // Initialize the LED_BUILTIN pin as an output

    // Initialize the MPU6050
    mpu.initialize();
}

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

    readSensorData(); // Read data from the MPU6050 sensor
    updateOutputs(); // Refresh output data

    // USER CODE: Control the built-in LED
    digitalWrite(LED_BUILTIN, LOW);
    delay(1000);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(2000);
}

void updateOutputs()
{
    // Update PWM signals for servos
    servo1.write(servo1_Servomotor_PWMSignal_PIN_D6_rawData);
    servo2.write(servo2_Servomotor_PWMSignal_PIN_D7_rawData);
}

void readSensorData()
{
    // Read accelerometer and gyroscope data
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // Process the sensor data to control the servos
    // Example: Map accelerometer values to servo positions
    servo1_Servomotor_PWMSignal_PIN_D6_rawData = map(ax, -17000, 17000, SERVOMIN, SERVOMAX);
    servo2_Servomotor_PWMSignal_PIN_D7_rawData = map(ay, -17000, 17000, SERVOMIN, SERVOMAX);
}

/* END CODE */