#define L_max 200

#define L_min 150

#define L_a 0.5

#define filter_init 10

#define L_t 20

byte val = 0x00; //调节与控制命令字

int i, LoopCount = 0; //平衡点调整，PID各调整设定系数

int E_0 = 0, E_1 = 0, PWM_Out, Left_PWM, Right_PWM; //误差，PWM输出，左右电机PWM输出

double Kp = 0.0, Kd = 0.0; //PID系数

double Len_filter = 0, Len_0 = 180; //测距滤波，机械平衡距离

unsigned int TrigPin = 4, EchoPin = 5, Len_Echo = 0; //HC-SR04触发信号，回波检测，回波时间

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; //系统时间

//电机输出

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值超过255

  if(Right_MotorVol > 255) Right_MotorVol = 255; //防止PWM值超过255

  analogWrite(M_ENA, Left_MotorVol);

  analogWrite(M_ENB, Right_MotorVol);

}

//初始化

void setup() {

  Serial.begin(115200); 

  pinMode(M_ENA, OUTPUT); //电机控制

  pinMode(M_IN1, OUTPUT);

  pinMode(M_IN2, OUTPUT); 

  pinMode(M_ENB, OUTPUT);

  pinMode(M_IN3, OUTPUT);

  pinMode(M_IN4, OUTPUT);   

  pinMode(EchoPin, INPUT); //超声波测距

  pinMode(TrigPin, OUTPUT);

  digitalWrite(TrigPin, LOW);

  i = 0;

  systime0 = millis();

}

//主循环程序

void loop() {

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

    systime0 = millis();

    digitalWrite(TrigPin, HIGH); //发送超声波测量触发脉冲

    delayMicroseconds(15);

    digitalWrite(TrigPin, LOW);

    Len_Echo = pulseIn(EchoPin, HIGH); //回波时间测量

    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;

    }

  }

  //处理串口指令，发送相应数据

  if (Serial.available() > 0) {

    val = Serial.read();

    //参数调节

    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); //查看设置参数

    }

    if(val == 0x08) {

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

    }

  }

}