Line Tracer

이거 학기초에 만든다는 얘기들었을 때는 재미있을것 같더니만,
교수님은 그냥 떡하니 핸디보드만 넘겨주고 뭘 자꾸 하라시니..
C언어도 안배워서 소스 봐도 모르겠구만..

아무튼 어떻게 어떻게 해서 소스만들었는데 잘 되려나..
센서는 발광부와 수광부가 합쳐져 있고 sg-2bc라는 모델이다.
6개 중에 앞에 5개 뒤에 1개를 달았다.

지금 소스는 앞 줄의 센서가 방향을 잡고,
뒤의 센서는 라인 이탈시에 라인으로 들어오도록 했다.

그런데 아래 그림 그리면서 알았는데
cb 센서가 라인 이탈시에 후진을 하면 안될 것 같다.

좀 더 생각해 보고 모르겠으면
그냥 직진과 종착지에서 멈추는데에만 써야겠다.

또 생각났다.
좌,우회전 시에 한쪽 바퀴를 멈추게 했는데
이렇게 하면 라인을 벗어날 일은 적지만
방향 잡고 조금 직진하고, 다시 방향 잡고 조금 직진하는 일이 반복되기 때문에
시간이 너무 오래 걸릴 것 같다.
한쪽 바퀴도 약간 움직이게 조절해야 한다.

invalid-file

int motl=0, motr=1; //motor left=0, motor right=1
int s=40; //motor speed
int ll=7, lr=6, cf=5, cb=4, rl=3, rr=2; //sensor
float dt=0.5; //delay time

int ll_notape, lr_notape, cf_notape, cb_notape, rl_notape, rr_notape; //sensor on no tape
int ll_tape, lr_tape, cf_tape, cb_tape, rl_tape, rr_tape; //sensor on tape
int ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver, d; //sensor values averge

void main(){
    calibrate();
    init();
    d=6;

    while(1){

      if (d==1) {

      while((analog(ll)<=ll_aver)&&(analog(lr)<=lr_aver)&&(analog(cf)>cf_aver)&&(analog(cb)>cb_aver)&&(analog(rl)<=rl_aver)&&(analog(rr)<=rr_aver)){

         motor(motl,0);

         motor(motr,2*s);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //speed left

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      if (d==2) {

      while((analog(ll)<=ll_aver)&&(analog(lr)<=lr_aver)&&(analog(cf)>cf_aver)&&(analog(cb)>cb_aver)&&(analog(rl)<=rl_aver)&&(analog(rr)<=rr_aver)){

         motor(motl,0);

         motor(motr,s);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //left

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      if (d==3) {

      while((analog(ll)<=ll_aver)&&(analog(lr)<=lr_aver)&&(analog(cf)>cf_aver)&&(analog(cb)>cb_aver)&&(analog(rl)<=rl_aver)&&(analog(rr)<=rr_aver)){

         motor(motl,s/2);

         motor(motr,s/2);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //dec

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      if (d==4) {

      while((analog(ll)<=ll_aver)&&(analog(lr)<=lr_aver)&&(analog(cf)>cf_aver)&&(analog(cb)>cb_aver)&&(analog(rl)<=rl_aver)&&(analog(rr)<=rr_aver)){

         motor(motl,-s);

         motor(motr,-s);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //back

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      }

      if (d==5) {

      while((analog(ll)<=ll_aver)&&(analog(lr)<=lr_aver)&&(analog(cf)>cf_aver)&&(analog(cb)>cb_aver)&&(analog(rl)<=rl_aver)&&(analog(rr)<=rr_aver)){

         motor(motl,s);

         motor(motr,0);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //right

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      if (d==6) {

      while((analog(ll)<=ll_aver)&&(analog(lr)<=lr_aver)&&(analog(cf)>cf_aver)&&(analog(cb)>cb_aver)&&(analog(rl)<=rl_aver)&&(analog(rr)<=rr_aver)){

         motor(motl,2*s);

         motor(motr,0);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //speed right

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      while((analog(ll)<=ll_aver)||(analog(lr)<=lr_aver)||(analog(cf)>cf_aver)||(analog(cb)>cb_aver)||(analog(rl)<=rl_aver)||(analog(rr)<=rr_aver)){

         motor(motl,2*s);

         motor(motr,2*s);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //acc

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      while((analog(ll)>ll_aver)||(analog(lr)>lr_aver)||(analog(cf)>cf_aver)||(analog(cb)>cb_aver)||(analog(rl)>rl_aver)||(analog(rr)>rr_aver)){

         motor(motl,0);

         motor(motr,0);

         printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, cb_aver, rl_aver, rr_aver); //stop

         if (analog(ll)>ll_aver)

            d=1;

   if (analog(lr)>lr_aver)

            d=2;

         if (analog(cf)<=cf_aver)

            d=3;

   if (analog(cb)<=cb_aver)

            d=4;

         if (analog(rl)>rl_aver)

            d=5;

   if (analog(rr)>rr_aver)

            d=6;

         }

      }

    }

}

/********** Subroutines *******************************/

void test(){

      while((analog(cf)>=cf_tape)&(analog(cb)>=cb_tape)){

         motor(motl,s);

         motor(motr,s);

         printf("%d %d %d %d %d %d\n",analog(ll),analog(lr),analog(cf),analog(cb),analog(rl),analog(rr)); //straight

         }

      ao();

      while (analog(lr)>=lr_tape){

         motor(motl,-s);

         motor(motr,s);

         printf("%d %d %d %d %d %d\n",analog(ll),analog(lr),analog(cf),analog(cb),analog(rl),analog(rr)); //right

         }

      while (analog(rl)>=rl_tape){

         motor(motl,s);

         motor(motr,-s);

         printf("%d %d %d %d %d %d\n",analog(ll),analog(lr),analog(cf),analog(cb),analog(rl),analog(rr)); //left

         }

}

void calibrate(){

   int temp;

   while(!stop_button()){

      ll_notape=analog(ll);

      lr_notape=analog(lr);

      cf_notape=analog(cf);

      cb_notape=analog(cb);

      rl_notape=analog(rl);

      rr_notape=analog(rr);

      printf("No Tape: %d %d %d %d %d %d\n",ll_notape,lr_notape,cf_notape,cb_notape,rl_notape,rr_notape); //no tape value

      sleep(.1);

   }

   tone(800.,1.);

   while(!stop_button()){

      ll_tape=analog(ll);

      lr_tape=analog(lr);

      cf_tape=analog(cf);

      cb_tape=analog(cb);

      rl_tape=analog(rl);

      rr_tape=analog(rr);

      printf("Tape: %d %d %d %d %d %d\n",ll_tape,lr_tape,cf_tape,cb_tape,rl_tape,rr_tape); //tape value

      sleep(.1);

   }

   temp=(ll_tape-ll_notape)/2;

   ll_aver=ll_notape+temp;

   temp=(lr_tape-lr_notape)/2;

   lr_aver=lr_notape+temp;

   temp=(cf_tape-cf_notape)/2;

   cf_aver=cf_notape+temp;

   temp=(rl_tape-rl_notape)/2;

   rl_aver=rl_notape+temp;

   temp=(rr_tape-rr_notape)/2;

   rr_aver=rr_notape+temp;

   temp=(cb_tape-cb_notape)/2;

   cb_aver=cb_notape+temp; //sensor values average

   printf("aver: %d %d %d %d %d %d\n", ll_aver, lr_aver, cf_aver, rl_aver, rr_aver, cb_aver);

   tone(800.,1.);

}

void init(){

   printf("Press Start to  Go!\n");

   while(!start_button()){}

   tone(800.,1.);

   }

p.s. 위의 소스로는 작동이 불가합니다..;;