#define L_max 200

#define L_min 150

#define L_a 0.5

#define filter_init 10

#define L_t 20

byte val = 0x00; //調(diào)節(jié)與控制命令字

int i, LoopCount = 0; //平衡點(diǎn)調(diào)整，PID各調(diào)整設(shè)定系數(shù)

int E_0 = 0, E_1 = 0, PWM_Out, Left_PWM, Right_PWM; //誤差，PWM輸出，左右電機(jī)PWM輸出

double Kp = 0.0, Kd = 0.0; //PID系數(shù)

double Len_filter = 0, Len_0 = 180; //測(cè)距濾波，機(jī)械平衡距離

unsigned int TrigPin = 4, EchoPin = 5, Len_Echo = 0; //HC-SR04觸發(fā)信號(hào)，回波檢測(cè)，回波時(shí)間

unsigned int M_IN1 = 6, M_IN2 = 7, M_IN3 = 8, M_IN4 = 9; // L298:IN1-IN4

unsigned int M_ENA = 10, M_ENB = 11; // L298:ENA-ENB

unsigned long systime0; //系統(tǒng)時(shí)間

//電機(jī)輸出

void SetMotorVoltage(int Left_MotorVol, int Right_MotorVol) {

  if(Left_MotorVol >= 0) {

    digitalWrite(M_IN1, LOW);

    digitalWrite(M_IN2, HIGH);

  }else {

    digitalWrite(M_IN1, HIGH);

    digitalWrite(M_IN2, LOW);

    Left_MotorVol = -Left_MotorVol;

  }

  if(Right_MotorVol >= 0) {

    digitalWrite(M_IN3, LOW);

    digitalWrite(M_IN4, HIGH);

  }else {

    digitalWrite(M_IN3, HIGH);

    digitalWrite(M_IN4, LOW);

    Right_MotorVol = -Right_MotorVol;

  }  

  if(Left_MotorVol > 255) Left_MotorVol = 255; //防止PWM值超過(guò)255

  if(Right_MotorVol > 255) Right_MotorVol = 255; //防止PWM值超過(guò)255

  analogWrite(M_ENA, Left_MotorVol);

  analogWrite(M_ENB, Right_MotorVol);

}

//初始化

void setup() {

  Serial.begin(115200); 

  pinMode(M_ENA, OUTPUT); //電機(jī)控制

  pinMode(M_IN1, OUTPUT);

  pinMode(M_IN2, OUTPUT); 

  pinMode(M_ENB, OUTPUT);

  pinMode(M_IN3, OUTPUT);

  pinMode(M_IN4, OUTPUT);   

  pinMode(EchoPin, INPUT); //超聲波測(cè)距

  pinMode(TrigPin, OUTPUT);

  digitalWrite(TrigPin, LOW);

  i = 0;

  systime0 = millis();

}

//主循環(huán)程序

void loop() {

  if(millis() - systime0 >= L_t) {

    systime0 = millis();

    digitalWrite(TrigPin, HIGH); //發(fā)送超聲波測(cè)量觸發(fā)脈沖

    delayMicroseconds(15);

    digitalWrite(TrigPin, LOW);

    Len_Echo = pulseIn(EchoPin, HIGH); //回波時(shí)間測(cè)量

    if((Len_Echo < L_max) && (Len_Echo > L_min)) {

      Len_filter *= L_a; //一階濾波

      Len_filter += (1 - L_a) * Len_Echo;

      i ++;

      if(i > filter_init){

        i = 100;

        LoopCount ++;

        E_0 = Len_0 - Len_filter;

        PWM_Out = Kp * E_0 + Kd * (E_0 - E_1);

        E_1 = E_0;

        Left_PWM = PWM_Out;

        Right_PWM = PWM_Out;

        SetMotorVoltage(Left_PWM, Right_PWM);

      }

    }else {

      SetMotorVoltage(0, 0); //超出平衡范圍，停止PWM輸出

      i = 0; Len_filter = 0;

    }

  }

  //處理串口指令，發(fā)送相應(yīng)數(shù)據(jù)

  if (Serial.available() > 0) {

    val = Serial.read();

    //參數(shù)調(diào)節(jié)

    if(val == 0x01) Kp += 0.01;

    if(val == 0x02) Kp -= 0.01;

    if(val == 0x03) Kd += 0.01;

    if(val == 0x04) Kd -= 0.01;

    if(val == 0x05) Len_0 ++;

    if(val == 0x06) Len_0 --;    

    if(val == 0x07) {

      Serial.print(Len_0); Serial.print("        "); 

      Serial.print(Kp); Serial.print("        "); Serial.println(Kd); //查看設(shè)置參數(shù)

    }

    if(val == 0x08) {

      Serial.print(Len_filter); Serial.print("        "); Serial.println(PWM_Out); //

    }

  }

}