自平衡机器人DIY（二）

装好小车后，最重要的部分就是PID算法了。

推荐一个讲PID算法的博文：点击打开链接

以及PID参数的调试步骤：点击打开链接

还有Arduino PID Library的官网：点击打开链接

目前我只实现了站立的功能，蓝牙遥控还没加上去，等春节后了吧。

代码中使用了两套PID参数进行控制，Kp，Ki，Kd是控制平衡速度的。也就是小车往前倾，加速；后倾，减速这个过程。

但是仅靠一个PID控制会出现一个问题：

因为小车的平衡角度Setpoint是手动设定的，所以会有一个微小的误差。当小车以Setpoint为基准进行调整时，可能有一个细微的倾斜，从而导致小车一直向一边跑。

于是引入另一套速度的PID参数：Sp,Si,Sd。 Inputs 为一个变量speedcount，每次中断检查车轮的转向，正向+1，反向-1。这样，当小车持续向一个方向移动时，speedcount会越来越大，从而产生的Outputs也更大。于是对车轮加一个反向的电压，使小车尽量停在原地。

实现的时候，把Outputs的作用加到第一套PID的目标平衡角度Setpoint上，就可以实现效果。

现在小车基本可以站立在一个小范围内了。



代码

#include "PID_v1.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu(0x68);

#define center  0x7F

char flag = 0;
double delta_v = 10.0;
char num = 0;
double time;
double balance_angle = 2.2;
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 = 17, ki = 100.0, kd = 0.5;//需要你修改的参数 kp = 25, ki = 185.0, kd = 0.29;

double Setpoints, Inputs, Outputs;
double sp = 20, si = 0, sd = 0.0;//需要你修改的参数

unsigned char dl = 50, 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;
}
/*
Input: The variable we're trying to control (double)
Output: The variable that will be adjusted by the pid (double)
Setpoint: The value we want to Input to maintain (double)
Kp, Ki, Kd: Tuning Parameters. these affect how the pid will chage the output. (double>=0)
*/
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 + delta_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 + delta_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);
//Serial.begin(9600);
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)
{
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println("DMP ready! Waiting for first interrupt...");
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
Serial.print("DMP Initialization failed !!!!!!(code ");
Serial.print(devStatus);
Serial.println(")");
}

Setpoint = balance_angle;                              // gogal value
Setpoints = 0.0;                                       // gogal speed

myPID.SetTunings(kp, ki, kd);
myPID.SetOutputLimits(-245 + dl, 245 - dl);
myPID.SetSampleTime(5);                        // PID algorithm will evaluate each 5ms
myPID.SetMode(AUTOMATIC);

sPID.SetTunings(sp, si, sd);
sPID.SetOutputLimits(-60, 60);
sPID.SetSampleTime(200);
sPID.SetMode(AUTOMATIC);

attachInterrupt(1, speed, RISING);            // 设置中断
}

void loop()
{
if (!dmpReady)  return;                                    // wait for MPU interrupt or extra packet(s) available
if (!mpuInterrupt && fifoCount < packetSize) return;
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
}
else if (mpuIntStatus & 0x02)
{
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);   //从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。单位：弧度
angle = -ypr[2] * 180 / M_PI;                //提取Roll，转化为角度制
}

Inputs = speedcount;
sPID.Compute();

Setpoint = balance_angle - Outputs;

Input = angle;
myPID.Compute();

if (angle>40 || angle<-40)
Output = 0;

//////////////
flag = 0;
if (flag)                     // flag == 1 , 转弯
{
motort(Output);
flag = 0;
}
else {
motor(Output);

}

if (Serial.available() > 0) {
inByte = Serial.read();
}
if (inByte == 'w'){
kp += 0.5;
}
else if (inByte == 'q'){
kp -= 0.5;
}
else if (inByte == 'r'){
ki += 10;
}
else if (inByte == 'e'){
ki -= 10;
}
else if (inByte == 'y'){
kd += 0.01;
}
else if (inByte == 't'){
kd -= 0.01;
}
else if (inByte == 'i'){
dl += 1;
}
else if (inByte == 'u'){
dl -= 1;
}
else if (inByte == 's'){
sp += 0.1;
}
else if (inByte == 'a'){
sp -= 0.1;
}
else if (inByte == 'f'){
si += 1;
}
else if (inByte == 'd'){
si -= 1;
}
else if (inByte == 'h'){
sd += 0.01;
}
else if (inByte == 'g'){
sd -= 0.01;
}
else if (inByte == 'v'){
Setpoints += 2;
}
else if (inByte == 'b'){
Setpoints -= 2;
}
else if (inByte == 'n'){    //转弯？？
Setpoints += 2;
flag = 1;
}
else if (inByte == 'm'){    //转弯？？
Setpoints -= 2;
flag = 1;
}

inByte = 'x';

sPID.SetTunings(sp, si, sd);
myPID.SetTunings(kp, ki, kd);

num++;
if (num == 20)
{
num = 0;
Serial.print("Kp: ");
Serial.print(kp);
Serial.print(",i: ");
Serial.print(ki);
Serial.print(",d: ");
Serial.print(kd);
Serial.print(",dl: ");
Serial.print(dl);
Serial.print("  Sp: ");
Serial.print(sp);
Serial.print(",i:");
Serial.print(si);
Serial.print(",d:");
Serial.print(sd);
Serial.print(", angle:");
Serial.println(angle);
Serial.print("Output: ");
Serial.print(Output);
Serial.print( "  Outputs: ");
Serial.println(Outputs);
}

}

void speed()
{
if (digitalRead(6)){
speedcount += 5;
}
else
speedcount -= 5;
}