#include <Servo.h>
Servo Biteservo; //servomotor 를 정의합니다.
//
const int trigPin=6,echoPin=7;
int   distance=0;
//
void setup(){
  pinMode(13,OUTPUT); // sub board control
  //
  pinMode(trigPin,OUTPUT); // trigPin을 출력으로
  pinMode(echoPin,INPUT);  // echoPin을 입력이다.
  //
  Biteservo.attach(A0,1000,2000); //신호를 받을부분의 핀을 선정합니다. 1000,2000이 각도 조절하는거
  //
  Serial.begin(9600);
 
  while(!Serial);
  Serial.println("Grove-Serial MP3 Demo");
  Serial.println(
  "Input command: \r\n\r\n"
  "P[]play music by default index\r\n"
  "Pm[]play music in MP3 folder by index\r\n"
  "Pf[][]play music by specify folder and index\r\n"
  "p Pause\r\n"
  "R Resume\r\n"
  "N Next\r\n"
  "L Previous\r\n"
  "l Loop\r\n"
  "I Increase volume\r\n"
  "D Decrease volumern\r\n");
  delay(100);
 
}
//--------------------------------------------------------------------------------------
//
unsigned long t,t_trig=0;
//
void loop(){
  t=millis();
  if(t-t_trig>100){ t_trig=t;
    digitalWrite(trigPin,HIGH); delayMicroseconds(10); digitalWrite(trigPin,LOW);
    distance=pulseIn(echoPin,HIGH)/58.2;
    //Serial.print("Distance: "); Serial.print(distance); Serial.println("cm");
  }
  distanceservo_loop();
  
  //
  if(distance<30)digitalWrite(13,HIGH);
  else           digitalWrite(13,LOW);
}
 
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
 
unsigned long t_servo=0;
unsigned int  servo_cnt=0,pos=0;
//
void distanceservo_loop(){
  if(distance<30){
    if(t-t_servo>30){ t_servo=t;  //servo>30의 숫자가 속도 조절
      if(servo_cnt<100){ } // delay(1000);
      else if(servo_cnt<=280){ pos=servo_cnt-100; Biteservo.write(pos); } // for(pos=0; pos<180; pos+=1){ Biteservo.write(pos); delay(10); } //서보의 위치 0~180도 각을 정의합니다.
      else if(servo_cnt<=380){ } // delay(1000);
      else if(servo_cnt<=561){ pos=561-servo_cnt; Biteservo.write(pos); } // for(pos=180; pos>=1; pos-=1){ Biteservo.write(pos); delay(10); } // 180도에서 0도까지 내려갑니다.
      if(++servo_cnt>561)servo_cnt=0;
    }
  }
  else{ pos=0; servo_cnt=0; Biteservo.write(pos); Serial.println("AAAA"); } // 30 이상이면 0도로 이동
}