Servo Control with MPU6050 and Flex Sensor

This C++ program allows control of four servos based on values obtained from an MPU6050 accelerometer/gyroscope and a flex sensor. The program reads the sensor values, maps them to servo movements, and outputs data to the Serial Monitor.

Required Libraries

Make sure to include the following libraries for the program:

• Wire.h for I2C communication
• Servo.h for servo control

Overview of Functions

The main functionalities of the program are encapsulated within the standard Arduino functions setup() and loop(). Below is the annotated code including documentation and input validation.

Code Implementation

#include 
#include 

// Initialize servo objects
Servo servo_1;
Servo servo_2;
Servo servo_3;
Servo servo_4;

// MPU6050 I2C address
const int MPU_addr = 0x68;

// Variables for raw sensor data
int16_t axis_X, axis_Y, axis_Z;
int minVal = 265;
int maxVal = 402;
double x, y, z;

// Setup function
void setup() {
    Serial.begin(9600); // Start serial communication at 9600 baud

    // Initialize I2C communication
    Wire.begin();
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x6B); // Wake up MPU6050
    Wire.write(0);
    Wire.endTransmission(true);

    // Attach servos to specified pins
    servo_1.attach(2);
    servo_2.attach(3);
    servo_3.attach(4);
    servo_4.attach(5);
}

// Main loop function
void loop() {
    // Read accelerometer/gyroscope values
    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true);

    // Extract sensor data
    axis_X = Wire.read() << 8 | Wire.read();
    axis_Y = Wire.read() << 8 | Wire.read();
    axis_Z = Wire.read() << 8 | Wire.read();

    // Map the raw data to servo angles
    int xAng = map(axis_X, minVal, maxVal, -90, 90);
    int yAng = map(axis_Y, minVal, maxVal, -90, 90);
    int zAng = map(axis_Z, minVal, maxVal, -90, 90);

    // Calculate angle positions from sensor data
    x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI);
    y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI);
    z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);

    // Read flex sensor value
    int flex_sensorip = analogRead(A0);
    int gripper = (flex_sensorip > 750) ? 0 : 180; // Control the gripper
    servo_3.write(gripper);

    // Control servo movements based on calculated angles
    if (x >= 0 && x <= 60) {
        int mov1 = map(x, 0, 60, 0, 90);
        Serial.print("Движение вперед/назад = ");
        Serial.print(mov1);
        Serial.println((char)176); // Print degree symbol
        servo_1.write(mov1);
    } else if (x >= 300 && x <= 360) {
        int mov2 = map(x, 360, 250, 0, 180);
        Serial.print("Движение вверх/вниз = ");
        Serial.print(mov2);
        Serial.println((char)176);
        servo_2.write(mov2);
    }

    if (y >= 0 && y <= 60) {
        int mov3 = map(y, 0, 60, 90, 180);
        Serial.print("Движение влево = ");
        Serial.print(mov3);
        Serial.println((char)176);
        servo_4.write(mov3);
    } else if (y >= 300 && y <= 360) {
        int mov3 = map(y, 360, 300, 90, 0);
        Serial.print("Движение вправо = ");
        Serial.print(mov3);
        Serial.println((char)176);
        servo_4.write(mov3);
    }
}

Usage Example

1. Hardware Setup: Connect the MPU6050 and flex sensor to appropriate pins on an Arduino board.
2. Upload the Program: Use the Arduino IDE to upload the provided code to the board.
3. Monitor Movement: Open the Serial Monitor in the IDE to see servo movements based on the accelerometer and flex sensor readings.

Conclusion

This program exemplifies how to interface with physical sensors and control servos based on the gathered data in an efficient and clear manner. Remember to validate sensor and actuator connections for accurate functionality.

Additional Learning

For deeper understanding and advanced topics on data handling, consider exploring the Enterprise DNA Platform's specialized courses for data scientists.