1. 小車平衡時(shí)會(huì)不斷的旋轉(zhuǎn)漂移布蔗，這是機(jī)械原因?qū)е碌膬奢喫俣炔灰恢绿傥ィ靡粋€(gè)速度控制小車平衡，兩輪因?yàn)檗D(zhuǎn)速不同就會(huì)偏轉(zhuǎn)纵揍，這時(shí)導(dǎo)入一個(gè)兩輪差速環(huán)顿乒，能分別控制兩輪轉(zhuǎn)速，使偏轉(zhuǎn)現(xiàn)象改觀泽谨；

2. 差速環(huán)是個(gè)原始的兩輪控制璧榄，在一個(gè)統(tǒng)一速度上，兩輪有一個(gè)差速吧雹，只能保證轉(zhuǎn)向后大概率回正骨杂，離工控級(jí)別的差速還很遠(yuǎn)，有時(shí)一個(gè)輪子不動(dòng)另一個(gè)使勁回正吮炕；

3. 全部代碼：

#include <PinChangeInterrupt.h>
#include <MPU6050_tockn.h>
#include <Wire.h>

int STBY = 8; //使能端
int PWMA = 9; //右電機(jī)PWM輸出控制腳   
int AIN1 = 7; //右電機(jī)正極  
int AIN2 = 6; //右電機(jī)負(fù)極  
int PWMB = 10; //左電機(jī)PWM輸出控制腳
int BIN1 = 13; //左電機(jī)正極 
int BIN2 = 12; //左電機(jī)負(fù)極  
#define PinA_left 4  //中斷 
#define PinA_right 2 //中斷1
int count_right = 0,count_left = 0;
float leftSpeed = 0,rightSpeed = 0,speed = 0,angleX = 0;
float leftOutput=0,rightOutput=0;
int zeroAngle = -1;                                                                //小車初始平衡角度,需要微調(diào)
float angleKp = 11,angleKd = 0.4 ,anglePreError=0,angleError=0,angleOutput=0;      //需要調(diào)整的角度環(huán)參數(shù)Kp,Kd
float speedKp = 2.2,speedKi = 0.2,speedTotalError=0,speedError=0,speedOutput=0;    //需要調(diào)整的速度環(huán)參數(shù)Kp,Ki
float turnKp = 2,turnKi = 0.1,turnTotalError=0,turnError=0,turnOutput=0;         //需要調(diào)整的轉(zhuǎn)向環(huán)參數(shù)Kp,Ki
float speedTarget = 0;                                                             //負(fù)為向前腊脱，正為向后，絕對(duì)值不要超過3
float turnTarget  = 0;                                                             //轉(zhuǎn)向角度
unsigned long now=0,past=0,interval=50;                                            //interval為小車調(diào)整姿態(tài)的時(shí)長(zhǎng),1000為一秒

MPU6050 mpu6050(Wire);

void setup(){  
  pinMode(STBY, OUTPUT);  
  pinMode(PWMA, OUTPUT);  
  pinMode(AIN1, OUTPUT);  
  pinMode(AIN2, OUTPUT);  
  pinMode(PWMB, OUTPUT);  
  pinMode(BIN1, OUTPUT);  
  pinMode(BIN2, OUTPUT);  
  pinMode(PinA_left, INPUT);  //測(cè)速碼盤輸入
  pinMode(PinA_right, INPUT);
  attachPinChangeInterrupt(digitalPinToPCINT(PinA_left), Code_left, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PinA_right), Code_right, CHANGE);
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
}

void loop(){
  now = millis();
  if(now-past>interval){
    getAngleX();
    getSpeed();
    angleOutput = calcAngleOutput(zeroAngle,angleX);
    speedOutput = calcSpeedOutput(speedTarget,speed);
    turnOutput  = calcTurnOutput(turnTarget,leftSpeed,rightSpeed);
    leftOutput  = angleOutput-speedOutput+turnOutput;
    rightOutput = angleOutput-speedOutput-turnOutput;
    adjustLeftAction(leftOutput);
    adjustRightAction(rightOutput);
    past = now;
  }
}

void runset(int motor, int speed, int direction){
  digitalWrite(STBY, 1); 
  if(motor==1 && direction== 1){
    digitalWrite(AIN1,1);
    digitalWrite(AIN2,0);
    analogWrite(PWMA, speed); 
  }
  if(motor==2 && direction== 1){
    digitalWrite(BIN1,1);
    digitalWrite(BIN2,0);
    analogWrite(PWMB, speed); 
  }
    if(motor==1 && direction== 0){
    digitalWrite(AIN1,0);
    digitalWrite(AIN2,1);
    analogWrite(PWMA, speed); 
  }
  if(motor==2 && direction== 0){
    digitalWrite(BIN1,0);
    digitalWrite(BIN2,1);
    analogWrite(PWMB, speed); 
  }
}

void stop(){  
  digitalWrite(STBY, LOW);   
}  

void Code_left() {
  count_left ++;
} //左測(cè)速碼盤計(jì)數(shù)

void Code_right() {
  count_right ++;
} //右測(cè)速碼盤計(jì)數(shù)

void getSpeed()
{
  if (digitalRead(AIN1)==1 && digitalRead(AIN2)==0){
    rightSpeed = count_right;
  }
  else if (digitalRead(AIN1)==0 && digitalRead(AIN2)==1){
    rightSpeed = -count_right;
  }
  if (digitalRead(BIN1)==1 && digitalRead(BIN2)==0){
    leftSpeed = count_left;
  }
  else if (digitalRead(BIN1)==0 && digitalRead(BIN2)==1){
    leftSpeed = -count_left;
  }
  speed = (leftSpeed+rightSpeed)/2;
  count_left  = 0;
  count_right = 0;
}

void getAngleX(){
  mpu6050.update();
  angleX = mpu6050.getAngleX();
}

float calcAngleOutput(float target,float pt){
  angleError = target-pt;
  angleOutput = angleKp*angleError + angleKd*(angleError-anglePreError);
  anglePreError = angleError;
  return angleOutput;
}

float calcSpeedOutput(float speedTarget,float pt){
  speedError = speedTarget-pt;
  speedTotalError += speedError;
  speedTotalError = (abs(speedTotalError)>100)?0:speedTotalError;
  speedOutput = speedKp*speedError + speedKi*speedTotalError;
  return speedOutput;
}

float calcTurnOutput(float turnTarget,float leftSpeed,float rightSpeed){
  turnError = turnTarget-(leftSpeed-rightSpeed);
  turnTotalError += turnError;
  turnTotalError = (abs(turnTotalError)>100)?0:turnTotalError;
  turnOutput = turnKp*speedError + turnKi*speedTotalError;
  return turnOutput;
}

void adjustLeftAction(float leftOutput){
  if (leftOutput>0){
    leftOutput = (leftOutput>254)?0:leftOutput;
    runset(2, (int)abs(leftOutput), 1);
    }
  else{
    leftOutput = (leftOutput<-254)?0:leftOutput;
    runset(2, (int)abs(leftOutput), 0);
  }
}

void adjustRightAction(float rightOutput){
  if (rightOutput>0){
    rightOutput = (rightOutput>254)?0:rightOutput;
    runset(1, (int)abs(rightOutput), 1);
    }
  else{
    rightOutput = (rightOutput<-254)?0:rightOutput;
    runset(1, (int)abs(rightOutput), 0);
  }
}