/************************使用的頭文件********************************/
#include <PinChangeInt.h>    //外部中斷
#include <MsTimer2.h>        //定時(shí)中斷



#include "I2Cdev.h"  //I2C協(xié)議庫(kù)
#include "Wire.h"    //服務(wù)于I2C通信  
#include "MPU6050_6Axis_MotionApps20.h"http://MPU6050庫(kù)文件

/*******************************************************************/

/************************實(shí)例化*********************************************************/


MPU6050 mpu6050;                             //實(shí)例化一個(gè) MPU6050 對(duì)象，對(duì)象名稱(chēng)為 mpu6050

/**************************************************************************************/

/***************************全局變量*************************************************/
int16_t ax, ay, az, gx, gy, gz;  //MPU6050的三軸加速度和三軸陀螺儀（角速度）數(shù)據(jù)

         /**********引腳分配************/
#define AIN1 9   //TB6612FNG驅(qū)動(dòng)模塊控制信號(hào) 共6個(gè)
#define AIN2 8
#define BIN1 12
#define BIN2 13
#define PWMA 6
#define PWMB 10 // 11號(hào)腳是廢的

#define ENCODER_L_A 2  //編碼器采集引腳 每路2個(gè) 共4個(gè) ENCODER_L
#define ENCODER_L_B 3  //DIRECTION_L
#define ENCODER_R_A 4  //ENCODER_R
#define ENCODER_R_B 5  //DIRECTION_R 



        /********************************/

        
#define DIFFERENCE 2       //DIFFERENCE是一個(gè)衡量平衡小車(chē)電機(jī)和機(jī)械安裝差異的一個(gè)變量湃番。直接作用于輸出夭织，讓小車(chē)具有更好的一致性。
#define ZHONGZHI -8        //小車(chē)的機(jī)械中值
float Turn_Amplitude = 40; //轉(zhuǎn)向目標(biāo)幅度值 
        
int Balance_Pwm, Velocity_Pwm, Turn_Pwm;   //直立 速度 轉(zhuǎn)向環(huán)的PWM

int Motor1, Motor2;      //電機(jī)疊加之后的PWM
volatile long Velocity_L, Velocity_R = 0;   //左右輪編碼器數(shù)據(jù)
int Velocity_Left, Velocity_Right = 0;     //左右輪速度

int Angle ;      //用于顯示的 角度 的臨時(shí)變量




/**** PID 參數(shù) *********/
float Balance_Kp= 15,  Balance_Kd=0.5;      // 直立PD
float Velocity_Kp= -2.4,  Velocity_Ki= -0.012;    // 速控PI吠撮，參考 kp = 2, ki = kp / 200; 
float Turn_Kp = 2, Turn_Kd = 0.001;        // 轉(zhuǎn)向PD
/*******************/








/********************************下為閉環(huán)函數(shù)*********************************/
/**************************************************************************
函數(shù)功能：直立PD控制  作者：平衡小車(chē)之家
入口參數(shù)：角度尊惰、角速度
返回  值：直立控制PWM
**************************************************************************/

int balance(float Angle, float Gyro)
{
  float Bias;
  int balance;
  Bias = Angle - ZHONGZHI;   //===求出平衡的角度中值 和機(jī)械相關(guān)
  balance = Balance_Kp * Bias + Gyro * Balance_Kd; //===計(jì)算平衡控制的電機(jī)PWM  PD控制   kp是P系數(shù) kd是D系數(shù); 15 ,0.4
  return balance;
}

/**************************************************************************
函數(shù)功能：速度PI控制 作者：平衡小車(chē)之家
入口參數(shù)：左輪編碼器、右輪編碼器
返回  值：速度控制PWM
**************************************************************************/
int velocity(int encoder_left, int encoder_right)
{
  static float Velocity, Encoder_Least, Encoder, Movement;
  static float Encoder_Integral, Target_Velocity;
  if       ( Flag_Qian == 1) Movement = -300;
  else   if ( Flag_Hou == 1) Movement = 300;
  else    //這里是停止的時(shí)候反轉(zhuǎn)，讓小車(chē)盡快停下來(lái)
  {
    Movement = 0;
    if (Encoder_Integral > 300)   Encoder_Integral -= 200;
    if (Encoder_Integral < -300)  Encoder_Integral += 200;
  }
  //=============速度PI控制器=======================//
  Encoder_Least = (encoder_left + encoder_right) - 0;               //===獲取最新速度偏差==測(cè)量速度（左右編碼器之和）-目標(biāo)速度（此處為零）
  Encoder *= 0.7;                                                   //===一階低通濾波器
  Encoder += Encoder_Least * 0.3;                                   //===一階低通濾波器
  Encoder_Integral += Encoder;                                      //===積分出位移 積分時(shí)間：40ms
  Encoder_Integral = Encoder_Integral - Movement;                   //===接收遙控器數(shù)據(jù)弄屡，控制前進(jìn)后退
  if (Encoder_Integral > 10000)    Encoder_Integral = 10000;        //===積分限,控制最高速度
  if (Encoder_Integral < -10000) Encoder_Integral = -10000;         //===積分限幅
  
  Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki;                  //===速度PI控制
  // if (Turn_Off(KalFilter.angle, Battery_Voltage) == 1 || Flag_Stop == 1)    Encoder_Integral = 0;//小車(chē)停止的時(shí)候積分清零
  return Velocity;
}

/**************************************************************************
函數(shù)功能：轉(zhuǎn)向控制 作者：平衡小車(chē)之家
入口參數(shù)：Z軸陀螺儀
返回  值：轉(zhuǎn)向控制PWM
**************************************************************************/
int turn(float gyro)//轉(zhuǎn)向控制
{
  static float Turn_Target, Turn, Turn_Convert = 1; // Turn_Convert可以控制轉(zhuǎn)向的快慢
  
  if (1 == Flag_Left)             Turn_Target += Turn_Convert;  //根據(jù)遙控指令改變轉(zhuǎn)向偏差
  else if (1 == Flag_Right)       Turn_Target -= Turn_Convert;//根據(jù)遙控指令改變轉(zhuǎn)向偏差
  else Turn_Target = 0;
  if (Turn_Target > Turn_Amplitude)  Turn_Target = Turn_Amplitude; //===轉(zhuǎn)向速度限幅
  if (Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude;
  
  Turn = - Turn_Target * Turn_Kp  + gyro * Turn_Kd;         //===結(jié)合Z軸陀螺儀進(jìn)行PD控制
  return Turn;
}

/**************************************************************************
函數(shù)功能：賦值給PWM寄存器 作者：平衡小車(chē)之家
入口參數(shù)：左輪PWM戴卜、右輪PWM
返回  值：無(wú)
**************************************************************************/
void Set_Pwm(int moto1, int moto2)
{
  if (moto1 > 0)   {  digitalWrite(AIN1, HIGH);      digitalWrite(AIN2, LOW); } //TB6612的電平控制
  else            { digitalWrite(AIN1, LOW);       digitalWrite(AIN2, HIGH); }//TB6612的電平控制
  analogWrite(PWMA, abs(moto1)); //賦值給PWM寄存器
  
  if (moto2 < 0)  { digitalWrite(BIN1, HIGH);     digitalWrite(BIN2, LOW); }//TB6612的電平控制
  else       { digitalWrite(BIN1, LOW);     digitalWrite(BIN2, HIGH); }//TB6612的電平控制
  analogWrite(PWMB, abs(moto2));//賦值給PWM寄存器
}

/**************************************************************************
函數(shù)功能：限制PWM賦值  作者：平衡小車(chē)之家
入口參數(shù)：無(wú)
返回  值：無(wú)
**************************************************************************/
void Xianfu_Pwm(void)
{
  int Amplitude = 250;  //===PWM滿(mǎn)幅是255 限制在250
   if(Flag_Qian==1)  Motor2-=DIFFERENCE;  //DIFFERENCE是一個(gè)衡量平衡小車(chē)電機(jī)和機(jī)械安裝差異的一個(gè)變量。直接作用于輸出琢岩，讓小車(chē)具有更好的一致性投剥。
   if(Flag_Hou==1)   Motor2-=DIFFERENCE-2;
  if (Motor1 < -Amplitude) Motor1 = -Amplitude;
  if (Motor1 > Amplitude)  Motor1 = Amplitude;
  if (Motor2 < -Amplitude) Motor2 = -Amplitude;
  if (Motor2 > Amplitude)  Motor2 = Amplitude;
}

/********************************************************************************/


/**************************************************************************
函數(shù)功能：5ms控制函數(shù) 核心代碼 作者：平衡小車(chē)之家
入口參數(shù)：無(wú)
返回  值：無(wú)
**************************************************************************/
void control()
{
  static int Velocity_Count , Turn_Count, Display_Count;

  sei();//全局中斷開(kāi)啟
  
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  //獲取MPU6050陀螺儀和加速度計(jì)的數(shù)據(jù)
  KalFilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);          //通過(guò)卡爾曼濾波獲取角度
  Angle = KalFilter.angle;//Angle是一個(gè)用于顯示的整形變量

  //直立PD控制 控制周期5ms
  Balance_Pwm = balance(KalFilter.angle, KalFilter.Gyro_x); 

  //速度PI控制，控制周期40ms
  if (++Velocity_Count >= 8) 
  {
    Velocity_Left = Velocity_L;    Velocity_L = 0;  //讀取左輪編碼器數(shù)據(jù)担孔，并清零江锨，這就是通過(guò)M法測(cè)速（單位時(shí)間內(nèi)的脈沖數(shù)）得到速度。
    Velocity_Right = - Velocity_R;    Velocity_R = 0; //讀取右輪編碼器數(shù)據(jù)糕篇，并清零
    Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制啄育，控制周期40ms
    Velocity_Count = 0;
  }
  
  //轉(zhuǎn)向PD控制，控制周期20ms
  if (++Turn_Count >= 4)
  {
    Turn_Pwm = turn(gz);
    Turn_Count = 0;
  }


  // 三環(huán)融合
  Motor1 = Balance_Pwm - Velocity_Pwm + Turn_Pwm;  //直立速度轉(zhuǎn)向環(huán)的疊加
  Motor2 = Balance_Pwm - Velocity_Pwm - Turn_Pwm; //直立速度轉(zhuǎn)向環(huán)的疊加

/*****************************************************/ 
  Xianfu_Pwm(); //限幅

  // 角度保護(hù)
  if(Angle > 40 || Angle < -50) Motor1=0,Motor2=0;

 
  Set_Pwm(Motor1, Motor2); //如果不存在異常拌消，賦值給PWM寄存器控制電機(jī)
  
  //屏幕顯示挑豌，每1500ms刷新一次
  if(OLED_F % 2 == 1){
    if(++Display_Count >= 300){
      Show_Screen();
      Display_Count = 0;
    }
  }
}

/**************************************************************************
函數(shù)功能：初始化，只執(zhí)行一次
入口參數(shù)：無(wú)
返回  值：無(wú)
**************************************************************************/
void setup() {
  // put your setup code here, to run once:
  pinMode(AIN1, OUTPUT);        //TB6612控制引腳墩崩，控制電機(jī)1的方向氓英，01為正轉(zhuǎn)，10為反轉(zhuǎn)
  pinMode(AIN2, OUTPUT);          //TB6612控制引腳鹦筹，
  pinMode(BIN1, OUTPUT);          //TB6612控制引腳铝阐，控制電機(jī)2的方向，01為正轉(zhuǎn)铐拐，10為反轉(zhuǎn)
  pinMode(BIN2, OUTPUT);          //TB6612控制引腳徘键，
  pinMode(PWMA, OUTPUT);         //TB6612控制引腳，電機(jī)PWM
  pinMode(PWMB, OUTPUT);         //TB6612控制引腳遍蟋，電機(jī)PWM
  digitalWrite(AIN1, 0);          //TB6612控制引腳拉低
  digitalWrite(AIN2, 0);          //TB6612控制引腳拉低
  digitalWrite(BIN1, 0);          //TB6612控制引腳拉低
  digitalWrite(BIN2, 0);          //TB6612控制引腳拉低
  analogWrite(PWMA, 0);          //TB6612控制引腳拉低
  analogWrite(PWMB, 0);          //TB6612控制引腳拉低

  pinMode(ENCODER_L_A, INPUT);       //編碼器引腳
  pinMode(ENCODER_L_B, INPUT);       //編碼器引腳
  pinMode(ENCODER_R_A, INPUT);       //編碼器引腳
  pinMode(ENCODER_R_B, INPUT);       //編碼器引腳


  Serial.begin(9600);       //開(kāi)啟串口吹害，設(shè)置波特率為 9600  
  Wire.begin();             //加入 IIC 總線(xiàn)
  delay(1500);              //延時(shí)等待初始化完成
  
  mpu6050.initialize();     //初始化MPU6050
  delay(20); 

  
  MsTimer2::set(5, control);  //使用Timer2設(shè)置5ms定時(shí)器中斷，
  MsTimer2::start();          //使用中斷使能虚青，Timer2開(kāi)始計(jì)時(shí)它呀，每5ms進(jìn)入一次中斷程序control

   /*中斷觸發(fā)類(lèi)型 
   LOW: 低電平觸發(fā)。
   CHANGE:管腳狀態(tài)改變觸發(fā)挟憔。
   RISING:上升沿觸發(fā)钟些。
   FALLING:下降沿觸發(fā)
  */
 
  /*開(kāi)啟外部中斷 編碼器接口1，0代表中斷號(hào)绊谭；
   ATmega168 / 328上有兩個(gè)外部中斷引腳，稱(chēng)為INT0和INT1汪拥。 INT0和INT1分別映射到引腳2和3达传；
   #define ENCODER_L_A 2 ,因此可以觸發(fā)左輪測(cè)速函數(shù)READ_ENCODER_L
   */
  attachInterrupt(0, READ_ENCODER_L, CHANGE);    

  /*開(kāi)啟外部中斷（引腳變化中斷） 編碼器接口2；
   引腳變化中斷可以在任何引腳上激活；
   #define ENCODER_R_A 4 ,因此可以觸發(fā)右輪測(cè)速函數(shù)READ_ENCODER_R
  */
  attachPinChangeInterrupt(4, READ_ENCODER_R, CHANGE);

  
}

/**************************************************************************
函數(shù)功能：主循環(huán)程序體
入口參數(shù)：無(wú)
返回  值：無(wú)
**************************************************************************/
void loop() {

  


}

/**************************************************************************
函數(shù)功能：外部中斷讀取編碼器數(shù)據(jù)宪赶，具有二倍頻功能 注意外部中斷是跳變沿觸發(fā)
入口參數(shù)：無(wú)
返回  值：無(wú)
**************************************************************************/
void READ_ENCODER_L() {
//  Serial.println("L");
  if (digitalRead(ENCODER_L_A) == LOW) {     //如果是下降沿觸發(fā)的中斷
    if (digitalRead(ENCODER_L_B) == LOW)      Velocity_L--;  //根據(jù)另外一相電平判定方向
    else      Velocity_L++;
  }
  else {     //如果是上升沿觸發(fā)的中斷
    if (digitalRead(ENCODER_L_B) == LOW)      Velocity_L++; //根據(jù)另外一相電平判定方向
    else     Velocity_L--;
  }
}
/**************************************************************************
函數(shù)功能：外部中斷讀取編碼器數(shù)據(jù)宗弯，具有二倍頻功能 注意外部中斷是跳變沿觸發(fā)
入口參數(shù)：無(wú)
返回  值：無(wú)
**************************************************************************/
void READ_ENCODER_R() {
//  while(1) Serial.println("R");
  if (digitalRead(ENCODER_R_A) == LOW) { //如果是下降沿觸發(fā)的中斷
    if (digitalRead(ENCODER_R_B) == LOW)      Velocity_R--;//根據(jù)另外一相電平判定方向
    else      Velocity_R++;
  }
  else {   //如果是上升沿觸發(fā)的中斷
    if (digitalRead(ENCODER_R_B) == LOW)      Velocity_R++; //根據(jù)另外一相電平判定方向
    else     Velocity_R--;
  }
}