#소스코드

#define H 1

#define L 0

#define trigpin 11

#define echopin 12

#define delay_time 990

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() {

  //pinMode(4, OUTPUT);   //A

  //pinMode(5, OUTPUT);   //B

  //pinMode(6, OUTPUT);   //An

  //pinMode(7, OUTPUT);   //Bn

  //pinMode(8, OUTPUT);   //enable1

  //pinMode(9, OUTPUT);   //enable2

  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(9600); //보레이트 설정

}

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(!flag){

    r_Stepping();  

    if(deg < 52)

      deg = deg + 1; 

    else

      flag = 1;    

  }

  else {

    l_Stepping();  

    if(deg > 1)

      deg = deg - 1; 

    else 

      flag = 0;          

  }

}