/*
* 电机1、2
*/
int EA = 10;  
int MA = 12;
int EB = 11;  
int MB = 13;

int mp3Echo = 7;

int trigPin1 = 6;    
int echoPin1 = 5;

int trigPin2 = 3;    
int echoPin2 = 2;

int count = 0;
int finalcount = 0;
bool state = false;

long duration1, dist1, average1;  
long aver1[3];   //array for average

long duration2, dist2, average2;  
long aver2[3];   //array for average


void setup() {  
  //初始化串口通信以及连接SR04的引脚    
    Serial.begin(9600);
    pinMode(MA, OUTPUT);  
    pinMode(MB, OUTPUT);
    pinMode(trigPin1, OUTPUT);   //要检测引脚上输入的脉冲宽度，需要先设置为输入状态  
    pinMode(echoPin1, INPUT);  

    pinMode(trigPin2, OUTPUT);   //要检测引脚上输入的脉冲宽度，需要先设置为输入状态  
    pinMode(echoPin2, INPUT);

    /*
      * mp3初始化
      */
    pinMode(mp3Echo,OUTPUT);
    digitalWrite(mp3Echo,HIGH);
}

void measure1() {  
   
    digitalWrite(trigPin1, HIGH);  //产生一个10us的高脉冲去触发trigPin1
    delayMicroseconds(3);
    digitalWrite(trigPin1, LOW);
    delayMicroseconds(6);
    digitalWrite(trigPin1, HIGH);
   
    pinMode(echoPin1, INPUT);
    duration1 = pulseIn(echoPin1, HIGH);  
    dist1 = (duration1/2) / 10;    //obtain distance1 检测脉冲宽度并测算出距离    
}

void measure2() {  
   
    digitalWrite(trigPin2, HIGH);  //产生一个10us的高脉冲去触发trigPin2
    delayMicroseconds(2);
    digitalWrite(trigPin2, LOW);
    delayMicroseconds(5);
    digitalWrite(trigPin2, HIGH);
   
    pinMode(echoPin2, INPUT);
    duration2 = pulseIn(echoPin2, HIGH);  
    dist2 = (duration2/2) / 10;    //obtain distance2 检测脉冲宽度并测算出距离    
}

void ultrasonic1(){
  for (int i=0;i<=2;i++) {   //average distance
    measure1();              
    aver1[i]=dist1;            
    delay(10);              //delay between measurements
  }
  dist1=(aver1[0]+aver1[1]+aver1[2])/3;  
 
}

void ultrasonic2(){
  for (int i=0;i<=2;i++) {   //average distance
    measure2();              
    aver2[i]=dist2;            
    delay(10);              //delay between measurements
  }
  dist2=(aver2[0]+aver2[1]+aver2[2])/3;  
}

/*
* 电机1
*/
void dangA1(){
  int value;
  for(value = 0 ; value <= 150; value+=5)
  {
    digitalWrite(MA,HIGH);  
    digitalWrite(MB, HIGH);
    analogWrite(EA, value);   //PWM调速
    analogWrite(EB, value);   //PWM调速
    delay(30);
  }
  delay(10000);
}

void dangA2(){
  int value;
  for(value = 0 ; value <= 200; value+=5)
  {
    digitalWrite(MA,HIGH);  
    digitalWrite(MB, HIGH);
    analogWrite(EA, value);   //PWM调速
    analogWrite(EB, value);   //PWM调速
    delay(30);
  }
  delay(10000);
}

void dangA3(){
  int value;
  for(value = 0 ; value <= 150; value+=5)
  {
    digitalWrite(MA,HIGH);  
    digitalWrite(MB, HIGH);
    analogWrite(EA, value);   //PWM调速
    analogWrite(EB, value);   //PWM调速
    delay(30);
  }
  delay(10000);
}

void stopMA(){
  digitalWrite(MA,LOW);    
  analogWrite(EA, 0);   //PWM调速
}

void stopMB(){
  digitalWrite(MB,LOW);    
  analogWrite(EB, 0);   //PWM调速
}

void loop() {
  ultrasonic1();
  ultrasonic2();
  Serial.print("1:");
  Serial.println(dist1);
  if(dist1 < 100){
    //Serial.print("1:");
    //Serial.println(dist1);
    state = true;
  }

  if(state)
    count++;

  if(count > 50)
    count = 0;
   
  if(dist2 < 100){
    Serial.print("2:");
    Serial.println(dist2);
    state = false;
    count = 0;
  }
  /*  
  if(count > 10){
    count = 0;
    dangA1();
    stopMA();
    stopMB();
    delay(10000);
  }
  if(count > 5 && count < 10){
    count = 0;
    dangA2();
    stopMA();
    stopMB();
    delay(10000);
  }*/
  if(count > 0 && count < 15){
    count = 0;
    digitalWrite(mp3Echo,LOW);
    delay(1000);
    dangA3();
    stopMA();
    stopMB();
    delay(10000);
  }
  digitalWrite(mp3Echo,HIGH);
  delay(50);
  state = false;
  count = 0;
}