两种Arduino环境下开环控制自平衡小车程序（只有直立环）

1.硬件电路

从左往右，依次是MPU6050陀螺仪模块，Promin 开发板，DRV8833电机驱动，两块升压板，将18650电池电压升高至5V。减小干扰，驱动电路与控制电路分离，所以用了两块升压板。依赖的库在代码头部说明里，代码是在前人基础上改的，文章最后有参考链接。

2.电机测试代码

#define rightMotorPWMPin   5
#define rightMotorDirPin   4
#define leftMotorPWMPin   6
#define leftMotorDirPin   7

int output = 200;

void Forward() //Code to rotate the wheel forward 
{
    digitalWrite(rightMotorDirPin, 1);
    digitalWrite(leftMotorDirPin, 1);
    analogWrite(rightMotorPWMPin,255-output);
    analogWrite(leftMotorPWMPin,255-output);
    
    Serial.println("FORWARD"); //Debugging information 
}

void Back() //Code to rotate the wheel forward 
{
    digitalWrite(rightMotorDirPin, 0);
    digitalWrite(leftMotorDirPin, 0);
    analogWrite(rightMotorPWMPin,output);
    analogWrite(leftMotorPWMPin,output);   
    Serial.println("BACK"); //Debugging information 
}

void TrunLeft() //Code to rotate the wheel forward 
{
    digitalWrite(rightMotorDirPin, HIGH);
    analogWrite(rightMotorPWMPin,output);
    analogWrite(leftMotorPWMPin,output);
    digitalWrite(leftMotorDirPin, LOW);
    Serial.println("LEFT"); //Debugging information 
}

void TrunRight() //Code to rotate the wheel forward 
{
    digitalWrite(rightMotorDirPin, LOW);
    analogWrite(rightMotorPWMPin,output);
    analogWrite(leftMotorPWMPin,output);
    digitalWrite(leftMotorDirPin, HIGH);
    Serial.println("LEFT"); //Debugging information 
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(leftMotorPWMPin, OUTPUT);
  pinMode(leftMotorDirPin, OUTPUT);
  pinMode(rightMotorPWMPin, OUTPUT);
  pinMode(rightMotorDirPin, OUTPUT);
 
}

void loop() {

  Forward();
  delay(3000);
  Back();
  delay(3000);
  
  /*

while(Serial.available()>0){

   char teststring = Serial.read();
   if(teststring=='a')
   {
    output=output+5;
    
    if(output>255)
    output=10;
    }

   }
   if(output!=output)
   {
   Serial.println(output);
   }
  Back();
  */
  /*
  while(Serial.available()>0){

   char teststring = Serial.read();

   switch (teststring) {
    case 'F':    
      Serial.println("Move Forward");
      Forward();
      break;
    case 'B':    
      Serial.println("Move Back"); 
      Back(); 
      break;
    case 'L':    
      Serial.println("TrunLeft");
      TrunLeft();
      break;
    case 'R':    
      Serial.println("TrunRight");
      TrunRight();
      break;
  }
    Serial.println(teststring);

   delay(1000); 
   
  
 
  }*/
}

3.MPU6050坐标轴

绕X轴旋转roll，横滚轴
绕Y轴旋转pitch，俯仰轴
绕Z轴旋转yaw，偏航角
这里采用的是Y轴与前后运动方向平行的安装方式，不一致的话，代码一改的代码量有点多，代码二改个数组就行。

4.MPU6050校准

// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW
// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net)

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//      2019-07-11 - added PID offset generation at begninning Generates first offsets 
//                 - in @ 6 seconds and completes with 4 more sets @ 10 seconds
//                 - then continues with origional 2016 calibration code.
//      2016-11-25 - added delays to reduce sampling rate to ~200 Hz
//                   added temporizing printing during long computations
//      2016-10-25 - requires inequality (Low < Target, High > Target) during expansion
//                   dynamic speed change when closing in
//      2016-10-22 - cosmetic changes
//      2016-10-19 - initial release of IMU_Zero
//      2013-05-08 - added multiple output formats
//                 - added seamless Fastwire support
//      2011-10-07 - initial release of MPU6050_RAW

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2011 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.

  If an MPU6050 
      * is an ideal member of its tribe, 
      * is properly warmed up, 
      * is at rest in a neutral p