自平衡機器人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;
}