Embedded Programming

#include 
#include 

#define MPU_ADDR 0x68
#define REG_PWR_MGMT_1 0x6B
#define REG_ACCEL_XOUT_H 0x3B

#define SERVO_X_PIN 32
#define SERVO_Y_PIN 33

const int frequency = 50;
const int resolution = 16;

void setup() {
  Serial.begin(115200);
  Wire.begin();

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(REG_PWR_MGMT_1);
  Wire.write(0x00);
  Wire.endTransmission(true);

  ledcAttach(SERVO_X_PIN, frequency, resolution);
  ledcAttach(SERVO_Y_PIN, frequency, resolution);
}

int angleADuty(int angle) {
  return map(angle, 0, 180, 1638, 8192);
}

void loop() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(REG_ACCEL_XOUT_H);
  Wire.endTransmission(false);
  
  Wire.requestFrom(MPU_ADDR, 4, true);

  if (Wire.available() >= 4) {
    int16_t ax = (Wire.read() << 8) | Wire.read();
    int16_t ay = (Wire.read() << 8) | Wire.read();

    float gX = ax / 16384.0;
    float gY = ay / 16384.0;

    // 3. Mapeamos a ángulos del Servo
    long angleX = map(ax, -16384, 16384, 0, 180);
    long angleY = map(ay, -16384, 16384, 0, 180);

    angleX = constrain(angleX, 0, 180);
    angleY = constrain(angleY, 0, 180);

    // 4. Movemos los servos
    ledcWrite(SERVO_X_PIN, angleADuty(angleX));
    ledcWrite(SERVO_Y_PIN, angleADuty(angleY));

    Serial.print("Axis X: ");
    Serial.print(gX, 2);      
    Serial.print(" g (");
    Serial.print(angleX);
    Serial.print("°) | Axis Y: ");
    Serial.print(gY, 2);
    Serial.print(" g (");
    Serial.print(angleY);
    Serial.println("°)");
  }
  
  delay(50);
}
                                

Explanation

I decided to use the Arduino framework as it is user-friendly and reduces the coding time required for basic functions.

Libraries and Definitions

• #include <Wire.h>: Includes the library to handle I2C communication.
• MPU_ADDR 0x68: The hexadecimal I2C address of the MPU6050 sensor.
• REG_PWR_MGMT_1: The internal register responsible for the sensor's power mode.
• REG_ACCEL_XOUT_H: The starting register address where the X-axis acceleration data is stored.

Pin & PWM Configuration

• const int frequency = 50;: Servomotors require a 50Hz signal (20ms period).
• const int resolution = 16;: We use 16-bit resolution for the PWM, giving us values from 0 to 65535 for very smooth movement precision.

Setup Function

• Wire.begin(): Initializes the I2C bus on the default ESP32 pins (GPIO 21 SDA, GPIO 22 SCL).
• >Waking up the sensor: The code sends a 0x00 byte to the power management register to wake the MPU6050 from sleep mode.
• ledcAttach(...): A specific ESP32 command. It connects the GPIO pin to the internal hardware timer to generate the PWM signal.

The Helper Function: Angle to Duty Cycle

This function converts a requested angle (0°-180°) into the specific Duty Cycle needed for the 16-bit PWM. Values 1638 and 8192 correspond to the pulse widths (approx 0.5ms to 2.5ms) required by the servo within the 50Hz cycle.

Main Loop

• Bitwise Operation (<< 8 |): The MPU6050 sends data in two separate 8-bit bytes (High and Low). This line combines them back into a single 16-bit signed integer.
• map(...): Converts the raw acceleration data (roughly -16384 to 16384 due to gravity) into servo angles.
• constrain(...): A safety function. It ensures the angle never goes below 0 or above 180, protecting the servo mechanism.
• ledcWrite(...): Updates the PWM duty cycle, physically moving the motor.

C++ Simulation