#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;
}
}
}
