给出一段基于arduino esp32的自平衡方块的代码

以下是一个基于 Arduino ESP32 的自平衡方块代码示例：

#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <PID_v1.h>

//定义电机
Servo motor1;
Servo motor2;

//定义MPU6050
Adafruit_MPU6050 mpu;

//PID参数
double Setpoint, Input, Output;
double Kp = 2, Ki = 5, Kd = 1;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  //初始化串口
  Serial.begin(115200);

  //初始化电机
  motor1.attach(14);
  motor2.attach(27);

  //初始化MPU6050
  if (!mpu.begin()) {
    Serial.println("MPU6050 not found");
    while (1);
  }

  //设置PID参数
  Setpoint = 0.0;
  pid.SetMode(AUTOMATIC);
  pid.SetSampleTime(10);
  pid.SetOutputLimits(-100, 100);
}

void loop() {
  //读取角度
  sensors_event_t event;
  mpu.getEvent(&event);
  Input = event.orientation.z;

  //计算PID输出
  pid.Compute();

  //控制电机
  int speed = map(Output, -100, 100, 0, 180);
  motor1.write(speed);
  motor2.write(180 - speed);
  
  //打印调试信息
  Serial.print("Input: ");
  Serial.print(Input);
  Serial.print(" Output: ");
  Serial.print(Output);
  Serial.print(" Speed: ");
  Serial.println(speed);

  delay(10);
}

该代码通过 MPU6050 模块读取角度信息，并通过 PID 算法计算控制电机的输出。其中，Kp、Ki 和 Kd 是 PID 控制算法中的参数，可以根据实际情况进行调整。在主循环中，需要调用 `pid.Compute()` 方法计算 PID 输出，并将其转换为电机转速进行控制。最终结果将在串口中打印出来，方便调试。