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



#include "I2Cdev.h"  //I2C協議庫
#include "Wire.h"    //服務于I2C通信  
#include "MPU6050_6Axis_MotionApps20.h"http://MPU6050庫文件

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

/************************實例化*********************************************************/


MPU6050 mpu6050;                             //實例化一個 MPU6050 對象，對象名稱為 mpu6050

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

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

         /**********引腳分配************/
#define AIN1 9   //TB6612FNG驅動模塊控制信號 共6個
#define AIN2 8
#define BIN1 12
#define BIN2 13
#define PWMA 6
#define PWMB 10 // 11號腳是廢的

#define ENCODER_L_A 2  //編碼器采集引腳 每路2個 共4個 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是一個衡量平衡小車電機和機械安裝差異的一個變量。直接作用于輸出，讓小車具有更好的一致性。
#define ZHONGZHI -8        //小車的機械中值
float Turn_Amplitude = 40; //轉向目標幅度值 
        
int Balance_Pwm, Velocity_Pwm, Turn_Pwm;   //直立 速度 轉向環的PWM

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

int Angle ;      //用于顯示的 角度 的臨時變量




/**** PID 參數 *********/
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;        // 轉向PD
/*******************/








/********************************下為閉環函數*********************************/
/**************************************************************************
函數功能：直立PD控制  作者：平衡小車之家
入口參數：角度、角速度
返回  值：直立控制PWM
**************************************************************************/

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

/**************************************************************************
函數功能：速度PI控制 作者：平衡小車之家
入口參數：左輪編碼器、右輪編碼器
返回  值：速度控制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    //這里是停止的時候反轉，讓小車盡快停下來
  {
    Movement = 0;
    if (Encoder_Integral > 300)   Encoder_Integral -= 200;
    if (Encoder_Integral < -300)  Encoder_Integral += 200;
  }
  //=============速度PI控制器=======================//
  Encoder_Least = (encoder_left + encoder_right) - 0;               //===獲取最新速度偏差==測量速度（左右編碼器之和）-目標速度（此處為零）
  Encoder *= 0.7;                                                   //===一階低通濾波器
  Encoder += Encoder_Least * 0.3;                                   //===一階低通濾波器
  Encoder_Integral += Encoder;                                      //===積分出位移 積分時間：40ms
  Encoder_Integral = Encoder_Integral - Movement;                   //===接收遙控器數據，控制前進后退
  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;//小車停止的時候積分清零
  return Velocity;
}

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

/**************************************************************************
函數功能：賦值給PWM寄存器 作者：平衡小車之家
入口參數：左輪PWM、右輪PWM
返回  值：無
**************************************************************************/
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寄存器
}

/**************************************************************************
函數功能：限制PWM賦值  作者：平衡小車之家
入口參數：無
返回  值：無
**************************************************************************/
void Xianfu_Pwm(void)
{
  int Amplitude = 250;  //===PWM滿幅是255 限制在250
   if(Flag_Qian==1)  Motor2-=DIFFERENCE;  //DIFFERENCE是一個衡量平衡小車電機和機械安裝差異的一個變量。直接作用于輸出，讓小車具有更好的一致性。
   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;
}

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


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

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

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

  //速度PI控制，控制周期40ms
  if (++Velocity_Count >= 8) 
  {
    Velocity_Left = Velocity_L;    Velocity_L = 0;  //讀取左輪編碼器數據，并清零，這就是通過M法測速（單位時間內的脈沖數）得到速度。
    Velocity_Right = - Velocity_R;    Velocity_R = 0; //讀取右輪編碼器數據，并清零
    Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制，控制周期40ms
    Velocity_Count = 0;
  }
  
  //轉向PD控制，控制周期20ms
  if (++Turn_Count >= 4)
  {
    Turn_Pwm = turn(gz);
    Turn_Count = 0;
  }


  // 三環融合
  Motor1 = Balance_Pwm - Velocity_Pwm + Turn_Pwm;  //直立速度轉向環的疊加
  Motor2 = Balance_Pwm - Velocity_Pwm - Turn_Pwm; //直立速度轉向環的疊加

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

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

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

/**************************************************************************
函數功能：初始化，只執行一次
入口參數：無
返回  值：無
**************************************************************************/
void setup() {
  // put your setup code here, to run once:
  pinMode(AIN1, OUTPUT);        //TB6612控制引腳，控制電機1的方向，01為正轉，10為反轉
  pinMode(AIN2, OUTPUT);          //TB6612控制引腳，
  pinMode(BIN1, OUTPUT);          //TB6612控制引腳，控制電機2的方向，01為正轉，10為反轉
  pinMode(BIN2, OUTPUT);          //TB6612控制引腳，
  pinMode(PWMA, OUTPUT);         //TB6612控制引腳，電機PWM
  pinMode(PWMB, OUTPUT);         //TB6612控制引腳，電機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);       //開啟串口，設置波特率為 9600  
  Wire.begin();             //加入 IIC 總線
  delay(1500);              //延時等待初始化完成
  
  mpu6050.initialize();     //初始化MPU6050
  delay(20); 

  
  MsTimer2::set(5, control);  //使用Timer2設置5ms定時器中斷，
  MsTimer2::start();          //使用中斷使能，Timer2開始計時，每5ms進入一次中斷程序control

   /*中斷觸發類型 
   LOW: 低電平觸發。
   CHANGE:管腳狀態改變觸發。
   RISING:上升沿觸發。
   FALLING:下降沿觸發
  */
 
  /*開啟外部中斷 編碼器接口1，0代表中斷號；
   ATmega168 / 328上有兩個外部中斷引腳，稱為INT0和INT1。 INT0和INT1分別映射到引腳2和3；
   #define ENCODER_L_A 2 ,因此可以觸發左輪測速函數READ_ENCODER_L
   */
  attachInterrupt(0, READ_ENCODER_L, CHANGE);    

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

  
}

/**************************************************************************
函數功能：主循環程序體
入口參數：無
返回  值：無
**************************************************************************/
void loop() {

  


}

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