Avoidance by Infrared and Ultrasonic(二)

在上一节中已经说明了要使用的超声波模块、红外避障模块和舵机模块的简单使用，这一节中就要将这些模块结合起来使用，实现其避障的功能

void infraredJudge(){
  while(1){
    turnForward();
    infraredLeftState = digitalRead(infraredLeft);
    infraredRightState = digitalRead(infraredRight);

    //obstacle on the left
    if(infraredLeftState == LOW && infraredRightState == HIGH){
      turnRight();
      delay(200);
    }
    //obstacle on the right
    else if(infraredLeftState == HIGH && infraredRightState == LOW){
      turnLeft();
      delay(200); 
    }
    //obstacle on the right and left
    else if(infraredLeftState == LOW && infraredRight == LOW){
      turnStop();
      delay(100);
      turnBack();
      delay(300);
      turnRight();
      delay(700); 
   }
   else

void distanceTestFront(){
  turnServo(servoAngleFront);
  delay(500);//make sure the servo reach specify position
  servoAngleCurrent = servoAngleFront;
  distanceFront = distanceTest();
}

void distanceTestLeft(){
  turnServo(servoAngleLeft);
  delay(500);
  servoAngleCurrent = servoAngleLeft;
  distanceLeft = distanceTest();
}

void distanceTestRight(){
  turnServo(servoAngleRight);
  delay(500);
  servoAngleCurrent = servoAngleRight;
  distanceRight = distanceTest();
}

void ultrasonicJudge(){
  while(1){
    turnForward();
    distanceTestFront();
    if(distanceFront < 20){
      turnStop();
      delay(100);
      turnBack();
      delay(200);
      turnStop();
      delay(100);
      distanceTestLeft();
      distanceTestRight();
      delay(100);
      if(distanceLeft < 20 && distanceRight < 20){
          turnRightOriginal();
          delay(700);
      }  
      else if(distanceLeft > distanceRight){
         turnLeft();
         delay(300);  
      }
      else if(distanceLeft < distanceRight){
        turnRight();
        delay(300);  
      }
      else

void infraUltraJudge(){
  while(1){
    turnForward();
    infraredLeftState  = digitalRead(infraredLeft);
    infraredRightState = digitalRead(infraredRight);
    if(infraredLeftState == LOW && infraredRightState == HIGH){
      turnRight();
      delay(150);
    }
    else if(infraredLeftState == HIGH && infraredRightState == LOW){
      turnLeft();
      delay(150);
    }
    else if(infraredLeftState == LOW && infraredRightState == LOW){
      turnStop();
      delay(200);
      distanceTestFront();
      if(distanceFront <= 20){
        distanceTestLeft();
        delay(500);
        distanceTestRight();
        delay(500);
        distanceTestFront();
        delay(500);
        if(distanceLeft <= 20 && distanceRight <= 20){
          turnBack();
          delay(400);
          turnRightOriginal();
          delay(800);
          turnStop();
          delay(100);
        }
        else if(distanceLeft > distanceRight){
          turnLeft();
          delay(500);
          turnStop();
          delay(100);
        }
        else if(distanceRight > distanceLeft){
          turnRight();
          delay(500);
          turnStop();
          delay(100);
        }
      }
    }
    else