[진행중-4]초음파센서-레이더(2)

#define H 1
#define L 0
#define trigpin 11
#define echopin 12
#define delay_time 10

int Motor[4] = {4,5,6,7};
int M_En[2] = {8,9};
int i, deg = 0;
int flag = 0;
float duration, distance;  

void setup() {
  for (i=0;i<4;i++){
    pinMode(Motor[i], OUTPUT);
  }
  for (i=0;i<2;i++){
    pinMode(M_En[i], OUTPUT);
  }  
  pinMode(trigpin, OUTPUT);   //Trig
  pinMode(echopin, INPUT);   //Echo 
  Serial.begin(115200); //보레이트 설정
}

void M_Enable(){
  for (i=0;i<2;i++){
    digitalWrite(M_En[i], H);
  }  
}

void M_Disable(){
  for (i=0;i<2;i++){
    digitalWrite(M_En[i], L);
  } 
}

void UltraSonic_command(){
/////////////////////////UltraSonic 동작 및 거리계산
////////////////////////////////////////////////// 대충 검색해서 퍼옴 - 구지... 이해할필요까지.. 없어보임.
  digitalWrite(trigpin, H);
  delay(10);
  digitalWrite(trigpin, L);
  duration = pulseIn(echopin, H);
  distance = ((float)(340*duration) / 10000) /2 ;
/////////////////////////시리얼로 프린트? TX 구문인지... 
  //Serial.print("distance : ");
  Serial.println(distance);
}

// 한스텝 동작 -> 거리측정 -> Serial Send
void M_move(int a, int b, int c, int d){
  M_Enable();
  digitalWrite(4, a);   // turn the LED on (HIGH is the voltage level)
  digitalWrite(5, b);   // turn the LED on (HIGH is the voltage level)
  digitalWrite(6, c);   // turn the LED on (HIGH is the voltage level)
  digitalWrite(7, d);   // turn the LED on (HIGH is the voltage level) 
  UltraSonic_command();
  M_Disable();  
}

// 오른쪽 동작
void r_Stepping(){  
  M_move(H,L,L,H);  
  delay(delay_time);                       // wait for a second
  M_move(H,H,L,L);  
  delay(delay_time);                       // wait for a second
  M_move(L,H,H,L);  
  delay(delay_time);                       // wait for a second
  M_move(L,L,H,H);  
  delay(delay_time);                       // wait for a second
}

// 왼쪽 동작
void l_Stepping(){  
  M_move(L,L,H,H);  
  delay(delay_time);                       // wait for a second
  M_move(L,H,H,L);    
  delay(delay_time);                       // wait for a second
  M_move(H,H,L,L);  
  delay(delay_time);                       // wait for a second
  M_move(H,L,L,H);  
  delay(delay_time);                       // wait for a second
}

// 오른쪽 360도 -> 왼쪽 360도 
void loop() { 
  if(Serial.available()>0){
    flag = Serial.read();
  }  
  if(flag){
    l_Stepping();  
    if(deg < 52)    
      deg = deg + 1; 
    else {
      flag = 0;          
      deg = 0;
    }
  }
}