Arduino平衡车——只角度环pid的代码

但是仅靠一个PID控制会出现一个问题：因为小车的平衡角度Setpoint是手动设定的，所以会有一个微小的误差。当小车以Setpoint为基准进行调整时，可能有一个细微的倾斜，从而导致小车一直向一边跑。建议引入另一套速度的PID参数：Sp,Si,Sd。

MPU6050接线
 SCL -- A5
 SDA -- A4
 INT -- PIN 2
 VCC -- 3.3V
 GND -- GND
*******************************************/

#include <PID_v1.h>
#include <LMotorController.h> //L298N控制库
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define MIN_ABS_SPEED 10//20

MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; //如果DMP初始化成功，设置为true
uint8_t mpuIntStatus; // 保存来自MPU的实际中断状态字节
uint8_t devStatus; // 每次设备操作后返回状态 (0 = success, !0 = error)
uint16_t packetSize; //预期DMP包大小(默认为42字节)
uint16_t fifoCount; // 当前FIFO中所有字节的计数
uint8_t fifoBuffer[64]; // FIFO存储缓冲区

//  定向运动/motion vars
Quaternion q; // [w, x, y, z] 四维
VectorFloat gravity; // [x, y, z] 重力三维矢量
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll 和重力矢量

//PID
double originalSetpoint = 183; //173, 调节小车的稳定中值, 180为默认值, 根据小车实际情况调试
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
//PID参数整定需要经过多次尝试, 首先调试P, 后D, 最后I
double Kp = 65;
double Kd = 1.2;
double Ki = 90;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

//电机速度偏差调整
double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.6;
//MOTOR CONTROLLER, L298N引脚定义
int ENA = 10;
int IN1 = 13;
int IN2 = 12;
int IN3 = 6;
int IN4 = 7;
int ENB = 9;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // 指示MPU中断引脚是否过高

void dmpDataReady()
{
  mpuInterrupt = true;
}


void setup()
{
  // 加入I2C总线(I2Cdev库不会自动这样做)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  Serial.begin(115200);
  delay(1000);
  Serial.println("Start!");

  mpu.initialize(); //初始化MPU6050
  devStatus = mpu.dmpInitialize();
   // 校正mpu6050模块
  mpu.setXGyroOffset(165);
  mpu.setYGyroOffset(-14);
  mpu.setZGyroOffset(5);
  mpu.setZAccelOffset(528); // 出厂默认
  // 确保它工作正常(如果是，返回0)
  if (devStatus == 0)
  {
    // turn on the DMP, now that it's ready
    mpu.setDMPEnabled(true);

    // 启用Arduino中断检测
    attachInterrupt(0, dmpDataReady, RISING); //开启INT0中断, mpu6050INT引脚需要接到arduino的PIN 2
    mpuIntStatus = mpu.getIntStatus();

    // 设置DMP就绪标志，以便main loop()函数知道可以使用它
    dmpReady = true;

    // 获得期望的DMP包大小以便稍后比较
    packetSize = mpu.dmpGetFIFOPacketSize();

    //setup PID
    //设置PID调节
    pid.SetMode(AUTOMATIC);
    pid.SetSampleTime(5); //调节间隔为5ms
    pid.SetOutputLimits(-250, 250); //PWM限速
  }
  else
  {
    // 错误！
    // 1 = 初始内存加载失败
    // 2 = DMP配置更新失败
    // (如果它要中断，通常代码是1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
}

void loop()
{
  // 如果编程失败了，就停在这里
  if (!dmpReady) return;

  // 等待MPU中断或额外数据包可用
  while (!mpuInterrupt && fifoCount < packetSize)
  {
    //没有mpu数据-执行PID计算和输出到电机
    pid.Compute();
    motorController.move(output, MIN_ABS_SPEED);
    Serial.print(output);
    Serial.print(",");
  }

  // 重置中断标志并获得INT_STATUS字节
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // 获取当前FIFO计数
  fifoCount = mpu.getFIFOCount();

  // 检查是否溢出(除非代码效率太低，否则不会发生)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    //重置
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));

    // 否则，检查DMP数据就绪中断(经常发生)
  }
  else if (mpuIntStatus & 0x02)
  {
    // 等待正确的可用数据长度
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // 从FIFO读取数据包
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // 跟踪FIFO计数在这里，以防有多余1个包可用
    // (可以帮助我们立即阅读更多内容而无需等待中断)
    fifoCount -= packetSize;

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    input = ypr[1] * 180 / M_PI + 180;
  }