自平衡小车控制代码
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
#include
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu(0x68);
#define center 0x7F
char flag=0;
char num=0;
double time;
signed int speeds = 0;
signed int oldspeed =0;
byte inByte ;
// MPU control/status vars
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
signed int speedcount=0;
// 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 float angle;
double Setpoint, Input, Output;
double kp = 18.8,ki = 185.0,kd = 0.29;//需要你修改的参数
double Setpoints, Inputs, Outputs;
double sp = 0.8,si = 0,sd = 0.22;//需要你修改的参数
unsigned char dl=17,count;
union{
signed int all;
unsigned char s[2];
}data;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT);
PID sPID(&Inputs, &Outputs, &Setpoints,sp,si,sd, DIRECT);
void motor(int v)
{
if(v>0)
{
v+=dl;
digitalWrite(6,0);
digitalWrite(7,1);
digitalWrite(8,1);
digitalWrite(9,0);
analogWrite(10,v);
analogWrite(11,v);
}
else if(v<0)
{
v=-v;
v+=dl;
digitalWrite(6,1);
digitalWrite(7,0);
digitalWrite(8,0);
digitalWrite(9,1);
analogWrite(10,v);
analogWrite(11,v);
}
else
{
analogWrite(10,0);
analogWrite(11,0);
}
}
void motort(int v)
{
if(v>0)
{
v+=dl;
digitalWrite(8,1);
digitalWrite(9,0);
analogWrite(10,v);
}
else if(v<0)
{
v=-v;
v+=dl;
digitalWrite(8,0);
digitalWrite(9,1);
analogWrite(10,v);
}
else
{
analogWrite(10,0);
}
}
void setup()
{
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
pinMode(8,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
pinMode(11,OUTPUT);
digitalWrite(6,0);
digitalWrite(7,1);
digitalWrite(8,1);
digitalWrite(9,0);
analogWrite(10,0);
analogWrite(11,0);
Serial.begin(115200);
Wire.begin();
delay(100);
Serial.println("Initializing I2C devices...");
mpu.initialize();
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(2);
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
if (devStatus == 0)