아두이노 관련 문의 드립니다

현재 졸작으로 음주측정잠금장치를 만드는데

저희가 만든 케이스에 잠금 장치를 만들어두고 그 위에 알콜센서를 두어 음주후 불었을때 일정수치이상이 되면 빨간불이 들어와 서브모터를 이용하여 잠금장치가 작동하지 않고 일정수치이하가 되면 파란불이 들어오고 잠금장치가 풀리는 것을 만들고 있습니다

현재 문제는 서브모터가 맘대로 작동하지가 않네요...

한번 봐주시고 문제점이나 해결책 좀 주세요~~

#include <Servo.h>

Servo myservo;     // 서보모터 객체 생성

int mq3Pin = A0;    // MQ-3 센서핀을 아두이노 보드의 A5 핀으로 설정
int redPin = 7;     // LED 센서의 R 핀을 아두이노 보드의 7번 핀으로 설정
int greenPin = 6;   // LED 센서의 G 핀을 아두이노 보드의 6번 핀으로 설정
int bluePin = 5;    // LED 센서의 B 핀을 아두이노 보드의 5번 핀으로 설정
int val = 0;        // 에탄올 센서 전역변수 설정
int piezo = 3;      // 부저 스피커 3번핀으로 설정
int motor = 9;      // 서보모터 2번 핀으로 설정
int angle = 90;     // 서보모터 초기 각도 값(90도)

void setup(){
  myservo.attach(motor);          // 서보모터 연결
  myservo.write(angle);
  Serial.begin(9600);          // 아두이노 보드와 PC간의 통신속도 설정 
  pinMode(redPin,OUTPUT);       // LED의 R,G,B 핀을 출력으로 설정
  pinMode(greenPin,OUTPUT);
  pinMode(bluePin,OUTPUT);
  pinMode(piezo, OUTPUT);       // 부저 스피커 출력설정
  delay(1000);
}

void loop(){
  val = analogRead(mq3Pin); 
  Serial.println(analogRead(mq3Pin));   // MQ-3 센서 출력값을 시리얼 모니터로 출력

  if(val>=400){                  // 센서 값이 400 이상이면
    if(val == 400){
      setColor(255, 0, 0);         // RGB 빨강
      Serial.print("400임");
      tone(piezo, 494);          // 부저 스피커 특정 조건이 만족하면 "시" 음으로 울림
      servo_ON();
      delay(500);
    }
  } 
  else if(val>=200 && val<400){  // 센서 값이 200 이상, 400 미만이면
    if(val <= 399){
      setColor(255, 255, 0);       // RGB 노랑
      Serial.print("300임");
      tone(piezo, 494);
      servo_ON();
      delay(500);
    }
  }
  else if(val<=200){               // 센서 값이 200 미만 이면
    if(val <= 200){                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        
      setColor(0, 0, 255);          // RGB 파랑
      Serial.print("200임");
      servo_ON();
      myservo.write(angle);
      delay(500);
    }
  }
   noTone(piezo);
   servo_OFF();
}

// RGB 색상만들기용
void setColor(int red, int green, int blue)
{
  analogWrite(redPin, red);
  analogWrite(greenPin, green);
  analogWrite(bluePin, blue);
}

void servo_ON(){
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo.write(90);
  myservo.detach(10000);
}

void servo_OFF(){
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo.write(0);
  myservo.detach(10000);
}