Mobile Roboter
Home Aufgabe Hardware Programme
 

 
Der Roboter sucht den Leuchtturm, fährt darauf zu und hält auf der schwarzen Fläche an

 
 

#include <tjpbase.h>
#include <stdio.h>

int geradeaus(int laenge);
void drehen(int grad);
void get_leuchtturm();

void main() {
   int boden_flag;

   init_analog();
   init_motortjp();
   init_clocktjp();
   IRE_ON;    


   START;  /*Press the rear bumper to start the program*/

  
   wait(2000);
   boden_flag = 0;
   while(!boden_flag) {
     get_leuchtturm();
     boden_flag = geradeaus(50); 
      
   }
}


int geradeaus(int laenge)
{  
  int irl,irr,boden,n=1;
  int wert=0;
  if (laenge < 0) {
     motorp(RIGHT_MOTOR,-MAX_SPEED);
     motorp(LEFT_MOTOR,-(MAX_SPEED*0.46)); /* 46 */
     laenge = laenge*(-1);
  } else {
     motorp(RIGHT_MOTOR,MAX_SPEED);
     motorp(LEFT_MOTOR,(MAX_SPEED*0.46)); /* 46 */
  }
  do {
    wait(400/9);

    boden = analog(7);
    if (boden > 90) {
          wert=1; 

    } 
  } while ((!wert) && (n++ < laenge));
  motorp(RIGHT_MOTOR,0);
  motorp(LEFT_MOTOR,0);  
  
  return wert;
}

void drehen(int grad)
{
  if (grad < 0) {
    motorp(RIGHT_MOTOR,-MAX_SPEED);
    motorp(LEFT_MOTOR,MAX_SPEED);
    grad = -grad;
    wait((int)((float)((1800/360)*grad)));
  } else {
    motorp(RIGHT_MOTOR,MAX_SPEED);
    motorp(LEFT_MOTOR,-MAX_SPEED);
    wait((int)((float)((1810/360)*grad)));
  }
  motorp(RIGHT_MOTOR,0);
  motorp(LEFT_MOTOR,0);  
}

void get_leuchtturm() {
  int leucht_null,leucht,leucht_max,winkel,i;
  int count=0;

  leucht_null = analog(5);
  leucht_max = 0;

  if (leucht_null < 255) {
     for (i=0; i < 36 ; i++) {
       leucht = analog(5);
       if(leucht > leucht_max) {
         leucht_max = leucht;
         winkel = i*10;
	   }
       drehen(10);
	 }
	 wait(500);
     drehen(winkel); 
  }
  wait(500);
  if ((leucht_max == 255) || (leucht_null == 255)) {

	 leucht = analog(5);

	 while (leucht == 255) {
		 drehen(-5);
		 leucht = analog(5);
	 }

	 drehen(10);
     count = 2;
	 leucht = analog(5);

	 while (leucht == 255) {
		 drehen(5);
		 leucht = analog(5);
         count++;
	 }

     winkel = (-1)*(count*5)/2;
	 drehen(winkel);

  }

}

 
  (C) by Jens Viebig, Tobias Arp, Niels Heitkämper in Y2K