用Arduino写一个平衡小车程序代码

以下是一个简单的平衡小车程序代码，使用MPU6050进行姿态测量，L298N电机驱动模块控制电机转动方向和速度：

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// 定义电机控制引脚
int motorPin1 = 3;
int motorPin2 = 5;
int motorPin3 = 6;
int motorPin4 = 9;

// 定义PID参数
float Kp = 20;  
float Ki = 0.05;  
float Kd = 12;  

// 定义全局变量
float angle = 0;  
float angle0 = 0;  
float angleSum = 0;  
float angleDiff = 0;  
float preAngle = 0;  

unsigned long sampleTime = 10;  

void setup() {
  // 初始化串口通信
  Serial.begin(9600);

  // 初始化MPU6050
  Wire.begin();
  mpu.initialize();

  // 设置电机控制引脚为输出模式
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
}

void loop() {
  // 读取MPU6050的角度
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float accY = ay / 16384.0;
  angle = atan(accY / sqrt(pow(ax / 16384.0, 2) + pow(az / 16384.0, 2))) * 180 / 3.1415926;

  // 计算PID控制量
  unsigned long currentTime = millis();
  if (currentTime - preTime >= sampleTime) {
    preTime = currentTime;
    angleDiff = angle - preAngle;
    angleSum += angle;
    float motorSpeed = Kp * angle + Ki * angleSum * sampleTime / 1000 + Kd * angleDiff * 1000 / sampleTime;
    preAngle = angle;

    // 控制电机转动
    if (motorSpeed > 0) {
      analogWrite(motorPin1, motorSpeed);
      analogWrite(motorPin2, 0);
      analogWrite(motorPin3, motorSpeed);
      analogWrite(motorPin4, 0);
    } else {
      analogWrite(motorPin1, 0);
      analogWrite(motorPin2, -motorSpeed);
      analogWrite(motorPin3, 0);
      analogWrite(motorPin4, -motorSpeed);
    }
  }

  // 输出调试信息
  Serial.print(angle);
  Serial.print("\t");
  Serial.println(motorSpeed);
}

注意：此代码仅为示例，具体实现需要根据实际情况进行调整和优化。