수다닷컴

터치센서 2개를 이용한 로봇입니다 이로봇으로 장애물을 통과 할려고 하는데 ----화살표 구간에 빠져서 빠져 나오질 못하더라구요 (참고로...그림은 잘 못그렷습니다ㅠ_ㅠ소질이 없어서) 저 화살표 구간에 빠지면 나올수 있는 명령어좀 알려주세요 ㅠ_ㅠ 계속 저구간에서 왓다 갓다만 하고 딜레이를 좀 죠서 뒤로 빠졌다가도 다시 앞으로 가서 빠져 버리고...어떤 명령어를 죠야 빠져 나올수 있는지 궁금합니다(참고 양옆은 막힌구간입니다 )
#include c:\winavr\ezrobomaster\ezrmlib.h
int main(void)
{
int Left=0, Right=0, count1=0, count2=0;
char str[10];

//초기화
SysInit();

//센서타입설정
LgsSetType(PORT_A6, LGS_TYPE_TOUCH);
LgsSetType(PORT_A7, LGS_TYPE_TOUCH);

while(1)
{

Left = LgsReadType(PORT_A6, LGS_TYPE_TOUCH);
Right = LgsReadType(PORT_A7, LGS_TYPE_TOUCH);

LgmSet(PORT_M1, DIR_CCW, 1);
LgmSet(PORT_M2, DIR_CW, 1);
LgmRun(PORT_M1+PORT_M2);

if(Left==1){

LedOn(LED_LEFT);
//부딪치면 멈추고
LgmStopFast(PORT_M1+PORT_M2);
Delay(500);
//모터 역으로 방향줘서 가게
LgmSet(PORT_M1, DIR_CW, 1);
LgmSet(PORT_M2, DIR_CCW, 1);
LgmRun(PORT_M1+PORT_M2);
Delay(600);
//다시 멈추고
LgmStopFast(PORT_M1+PORT_M2);
Delay(400);
//왼쪽이 부딪쳤으니 오른쪽으로 방향을 틀게
LgmSet(PORT_M1, DIR_CCW, 1);
LgmRun(PORT_M1);
Delay(400);
//멈추고
LgmStopFast(PORT_M1+PORT_M2);
Delay(400);
//약간 좌회전 전진
LgmSet(PORT_M1, DIR_CCW, 1);
LgmSet(PORT_M2, DIR_CW, 2);
LgmRun(PORT_M1+PORT_M2);
Delay(700);
//다시 직진
LgmSet(PORT_M1, DIR_CCW, 1);
LgmSet(PORT_M2, DIR_CW, 1);
LgmRun(PORT_M1+PORT_M2);
//무한루프 방지를 위해 카운트 1증가
count1++;
LedOff(LED_LEFT);
Delay(500);
}
if(Right==1){

LedOn(LED_RIGHT);
//부딪치면 멈추고
LgmStopFast(PORT_M1+PORT_M2);
Delay(500);
//모터 역으로 방향 줘서 거꾸로 가게
LgmSet(PORT_M1, DIR_CW, 1);
LgmSet(PORT_M2, DIR_CCW, 1);
LgmRun(PORT_M1+PORT_M2);
Delay(600);
//다시 멈추고
LgmStopFast(PORT_M1+PORT_M2);
Delay(400);
//오른쪽을 부딪쳤으니 왼쪽으로 방향을 틀게
LgmSet(PORT_M2, DIR_CW, 1);
LgmRun(PORT_M2);
Delay(400);
//멈추고
LgmStopFast(PORT_M1+PORT_M2);
Delay(400);
//약간 우회전 전진
LgmSet(PORT_M1, DIR_CCW, 2);
LgmSet(PORT_M2, DIR_CW, 1);
LgmRun(PORT_M1+PORT_M2);
Delay(700);
//다시 직진
LgmSet(PORT_M1, DIR_CCW, 1);
LgmSet(PORT_M2, DIR_CW, 1);
LgmRun(PORT_M1+PORT_M2);
//무한루프 방지를 위해 카운트 1증가
count2++;
LedOff(LED_RIGHT);
Delay(500);
}
//LCD 창에 카운트 숫자 표시
sprintf(str, count %2d, count1+count2);
LcdDspStr(0, 0, str);

//카운트9가 넘어가면 뒤로
if(count1+count2 == 9){
LgmStopFast(PORT_M1+PORT_M2);
Delay(300);
LgmSet(PORT_M1, DIR_CW, 1);
LgmSet(PORT_M2, DIR_CCW, 1);
LgmRun(PORT_M1+PORT_M2);
Delay(1000);
count1=0;
count2=0;
LgmStopFast(PORT_M1+PORT_M2);
Delay(300);
}

}
return 0;
}
골인할수 잇게 따른 추가 명령어나 고쳐야할 부분 잇으면 가르쳐 주세요 T^T

신청하기