Arduino 两台自平衡机器人

首先，让我们解释一下所使用的组件及其功能。

- 采用微机电系统（MEMS）技术的MPU6050加速度计和陀螺仪传感器，用于检测沿X、Y、Z轴的倾斜角度或倾斜角度。

- 来自该传感器的信息被传输到微控制器，在我们的例子中它是 Arduino Uno 板。该板是机器人的大脑，处理传感器数据并确定保持机器人直立所需的作。

- 来自 Arduino 的处理数据被发送到电机驱动板，在我们的例子中是 L298N 5AD 类型。电机驱动电路控制电机的方向和速度。

- 该机器人使用两个直流电机，每个电机都连接到一个轮子上。这些电机调节车轮的速度以纠正机器人的倾斜。

- 和电源，特别是串联的两节锂电池，为电机和电子设备提供能量。所有组件都安装在由 5 毫米厚的 PVC 板制成的合适支撑结构上。



现在，我们简单解释一下工作原理：加速度计测量机器人相对于重力的方向，陀螺仪检测倾斜率。Arduino 使用 PID（比例、积分、微分）控制器来处理来自传感器的数据。


PID 控制器计算保持机器人平衡所需的必要速度和方向调整。如果机器人向前倾斜，电机会旋转得更快，使机器人向前移动并防止跌倒。如果它向后倾斜，电机会向后移动机器人以恢复平衡。当机器人检测到不平衡时，它会通过调整轮速、移动质心或向前或向后移动以保持其位置来进行补偿。

关于 Arduino 代码的几句话。安装代码的方法之前已经描述过很多次了，所以现在我只关注由于所用组件的不同特性以及它们的放置方法，需要针对每种情况单独进行的修改。

现在是有趣的部分，即专门为我们构建的设备自定义代码。根据组件的布局，机器人的重心会发生变化，并且每个MPU6050板的差异很小，需要在部分代码中进行补偿。如果我们更详细地查看代码，我们会看到有一个位置设置了陀螺仪偏移量。


在代码的这一部分中，在MPU6050芯片的制造过程中纠正了错误。对于每个单独的芯片，这些值都是不同的。它们的值是使用专门为此目的制作的 Arduino 代码确定的，您可以在本文末尾下载它。

接下来，我们需要继续设置加速度计。在“双设定点”行中，我们需要输入一个与我们的设备相对应的值。



为了获得这个值，我们需要将机器人保持在垂直于表面的理想垂直位置，并在串行监视器上读取所需的值。就我而言，大约是 182
最后，我们需要通过实验确定PID控制所需的Kp、Kd和Ki值。

为此，我们将所有三个值都设置为零。现在我们为 Kp 设置一些值并执行测试。Kp 太少会使机器人摔倒。过多的 Kp 会让机器人来回疯狂。一个好的 Kp 会让机器人稍微来回移动。接下来我们需要设置 Kd。gooD Kd 值将减少振荡，直到机器人几乎稳定，并使机器人保持站立。最后设置气。正确的 Ki 值将缩短机器人稳定所需的时间。我想强调的是，表面需要足够粗糙，以增加它与车轮之间的摩擦力。

复制

#include "I2Cdev.h"

#include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h

#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

MPU6050 mpu;

// MPU control/status vars

bool dmpReady = false;  // set true if DMP init was successful

uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU

uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount;     // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q;           // [w, x, y, z]         quaternion container

VectorFloat gravity;    // [x, y, z]            gravity vector

float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

/*********Tune these 4 values for your BOT*********/

double setpoint= 182; //set the value when the bot is perpendicular to ground using serial monitor. 

//Read the project documentation on circuitdigest.com to learn how to set these values

double Kp = 15; //21 Set this first

double Kd = 0.9; //0.8 Set this secound

double Ki = 140; //140 Finally set this 

/******End of values setting*********/

double input, output;

PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady()

{

    mpuInterrupt = true;

}

void setup() {

  Serial.begin(115200);

  // initialize device

    Serial.println(F("Initializing I2C devices..."));

    mpu.initialize();

     // verify connection

    Serial.println(F("Testing device connections..."));

    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP

    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity

    mpu.setXGyroOffset(-479);

    mpu.setYGyroOffset(84);

    mpu.setZGyroOffset(15);

    mpu.setZAccelOffset(1638); 

      // make sure it worked (returns 0 if so)

    if (devStatus == 0)

    {

        // turn on the DMP, now that it's ready

        Serial.println(F("Enabling DMP..."));

        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection

        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));

        attachInterrupt(0, dmpDataReady, RISING);

        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it

        Serial.println(F("DMP ready! Waiting for first interrupt..."));

        dmpReady = true;

        // get expected DMP packet size for later comparison

        packetSize = mpu.dmpGetFIFOPacketSize();

        //setup PID

        pid.SetMode(AUTOMATIC);

        pid.SetSampleTime(10);

        pid.SetOutputLimits(-255, 255);

    }

    else

    {

        // ERROR!

        // 1 = initial memory load failed

        // 2 = DMP configuration updates failed

        // (if it's going to break, usually the code will be 1)

        Serial.print(F("DMP Initialization failed (code "));

        Serial.print(devStatus);

        Serial.println(F(")"));

    }

//Initialise the Motor outpu pins

    pinMode (6, OUTPUT);

    pinMode (9, OUTPUT);

    pinMode (10, OUTPUT);

    pinMode (11, OUTPUT);

//By default turn off both the motors

    analogWrite(6,LOW);

    analogWrite(9,LOW);

    analogWrite(10,LOW);

    analogWrite(11,LOW);

}

void loop() {

    // if programming failed, don't try to do anything

    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available

    while (!mpuInterrupt && fifoCount < packetSize)

    {

        //no mpu data - performing PID calculations and output to motors     

        pid.Compute();

        //Print the value of Input and Output on serial monitor to check how it is working.

        Serial.print(input); Serial.print(" =>"); Serial.println(output);

        if (input>150 && input<200){//If the Bot is falling 

        if (output>0) //Falling towards front 

        Forward(); //Rotate the wheels forward 

        else if (output<0) //Falling towards back

        Reverse(); //Rotate the wheels backward 

        }

        else //If Bot not falling

        Stop(); //Hold the wheels still

    }

    // reset interrupt flag and get INT_STATUS byte

    mpuInterrupt = false;

    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count

    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)

    if ((mpuIntStatus & 0x10) || fifoCount == 1024)

    {

        // reset so we can continue cleanly

        mpu.resetFIFO();

        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)

    }

    else if (mpuIntStatus & 0x02)

    {

        // wait for correct available data length, should be a VERY short wait

        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO

        mpu.getFIFOBytes(fifoBuffer, packetSize);

        // track FIFO count here in case there is > 1 packet available

        // (this lets us immediately read more without waiting for an interrupt)

        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q

        mpu.dmpGetGravity(&gravity, &q); //get value for gravity

        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

        input = ypr[1] * 180/M_PI + 180;

   }

}

void Forward() //Code to rotate the wheel forward 

{

    analogWrite(6,output);

    analogWrite(9,0);

    analogWrite(10,output);

    analogWrite(11,0);

    Serial.print("F"); //Debugging information 

}

void Reverse() //Code to rotate the wheel Backward  

{

    analogWrite(6,0);

    analogWrite(9,output*-1);

    analogWrite(10,0);

    analogWrite(11,output*-1);

    Serial.print("R");

}

void Stop() //Code to stop both the wheels

{

    analogWrite(6,0);

    analogWrite(9,0);

    analogWrite(10,0);

    analogWrite(11,0);

    Serial.print("S");

}

看起来不错，大家可以参考这个代码试试。