自平衡小车程序

[mw_shl_code=c,true][code][mw_shl_code=c,true][code] //@//@引入引入 Wire.hWire.h 头文件，下面的头文件，下面的 I2Cdev.hI2Cdev.h 依赖于它依赖于它 #include “Wire.h“#include “Wire.h“ //@I2Cdev//@I2Cdev 和和 MPU6050MPU6050 类库需要事先安装在类库需要事先安装在ArduinoArduino 类库文件夹下类库文件夹下 //@ //@ 引引入入 I2Cdev.hI2Cdev.h 头文件头文件 #include “I2Cdev.h“#include “I2Cdev.h“ //@//@引入引入 #include#include “MPU6050_6Axis_MotionApps20.h““MPU6050_6Axis_MotionApps20.h“ //@ //@实实例例化化一一个个 MPU6050MPU6050 对象，对象名称为对象，对象名称为 mpumpu MPU6050 mpu(0 x68);MPU6050 mpu(0 x68); float alpha,omiga,r1; //@float alpha,omiga,r1; //@声明声明 2 2 个浮点数变个浮点数变量，量， alphaalpha 和和 omigaomiga float kp=26.458;//float kp=26.458;//需要调节的值需要调节的值 float ki=2.37;//float ki=2.37;//需要调节的值需要调节的值 float kd=1.604;//float kd=1.604;//需要调节的值需要调节的值 int AN1=3;int AN1=3; int AN2=4;int AN2=4; int BN1=5;int BN1=5; int BN2=6;int BN2=6; int PWMA=9;//int PWMA=9;//电机电机 1 1 调速口调速口 int PWMB=10;//int PWMB=10;//电机电机 2 2 调速口调速口 //( //(注注意意：：以以下下程程序序都都是是为为了了得得到到 mpu6050mpu6050 测出的角度测出的角度) ) //@//@ 声明声明 MPU6050MPU6050 控制和状态变量控制和状态变量 bool dmpReady = false; //@set true if DMPbool dmpReady = false; //@set true if DMP init was successfulinit was successful uint8_tuint8_t mpuIntStatus;mpuIntStatus; //@ //@此变量用于保此变量用于保存存 MPU6050MPU6050 中断状态中断状态 uint8_t devStatus;uint8_t devStatus; //@ //@返回设备状态，返回设备状态，0 0 为成功，不为为成功，不为 0 0 则发生错误则发生错误 uint16_tuint16_t packetSize;packetSize;// // expectedexpected DMPDMP packet size (default is 42 bytes)packet size (default is 42 bytes) uint16_tuint16_t fifoCount;fifoCount; // // countcount ofof allall bytesbytes currently in FIFOcurrently in FIFO uint8_tuint8_tfifoBuffer[64];fifoBuffer[64];// //FIFOFIFOstoragestorage bufferbuffer //@//@声明方向和声明方向和运动变量：运动变量： QuaternionQuaternion q;q; //@ //@ 四四元元数数变变量量 W,X,Y,ZW,X,Y,Z VectorFloat gravity;VectorFloat gravity;//@//@重力矢量重力矢量 X X，，Y,Y, Z Z floatfloatypr[3];ypr[3]; // //[yaw,[yaw,pitch,pitch, roll]roll] yaw/pitch/roll yaw/pitch/roll containercontainer andand gravitygravity vectorvector // // ================================================================== ============================================================== // // ====================================================== 中断中断检测程序检测程序 ================================================== // // ================================================================== ============================================================== volatilevolatile boolbool mpuInterruptmpuInterrupt = = false;false; // // indicatesindicates whetherwhether MPUMPU interruptinterrupt pinpin hashas gone highgone high void dmpDataReady()void dmpDataReady() { { mpuInterrupt = true; mpuInterrupt = true; } } void setup()void setup() { { Serial.begin(115200); //@ Serial.begin(115200); //@开启串口，开启串口，设置波设置波特率为特率为 115200115200，，程序下载到程序下载到 ArduinoArduino 之后之后注意打开串口观察注意打开串口观察 pinMode(AN1,OUTPUT); pinMode(AN1,OUTPUT); pinMode(AN2,OUTPUT); pinMode(AN2,OUTPUT); pinMode(BN1,OUTPUT); pinMode(BN1,OUTPUT); pinMode(BN2,OUTPUT); pinMode(BN2,OUTPUT); pinMode(PWMA,OUTPUT); pinMode(PWMA,OUTPUT);