Der Roboter findet den Weg durch einen Gang ohne mit den Wänden zu kollidieren

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

int geradeaus(int laenge);
void drehen(int grad);
unsigned rand(int wert);
void collision();

void main() {
  int run = 1;
     
  init_analog();
  init_motortjp();
  init_clocktjp();
  IRE_ON;    


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

  
  wait(2000);
  while (run) {
    switch (geradeaus(100)) {
     case 1: drehen(10);
             break;
     case 2: drehen(-10);
             break;
     case 3: drehen(30);
             break;
     case 4: collision();
             break;
     case 5: run = 0;
             break;
    }
  }  
}  

/* Laenge in cm */
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);
    irl=LEFT_IR;
    irr=RIGHT_IR;

    if (((irl > 100) && (irr > 98)) || ((irl > 98) && (irr > 100))) {
       wert = 3;
    } else {
       if (irl > 100)   
          wert=2; 
       if (irr > 100)
          wert=1;
    } 
    if (BUMPER >= 10)
          wert=4;

  } 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 collision()
{    int bump;
     bump = BUMPER;
     if (bump < 110) {      
        motorp(RIGHT_MOTOR,-MAX_SPEED);
        motorp(LEFT_MOTOR,-(MAX_SPEED*0.46)); /* 46 */
        wait(200);
        if ((10 < bump) && (bump < 30))
           drehen(90);
        if ((30 < bump) && (bump < 50))
           drehen(180);
        if ((50 < bump) && (bump < 70))
           drehen(120);           
        if ((70 < bump) && (bump < 90))
           drehen(-90);
        if ((90 < bump) && (bump < 110))
           drehen(-120);            
     }
}
unsigned rand(int wert)
{
   unsigned ran;
   ran = TCNT;
   if ((ran & 0x0003) > wert)
      wert = 0;
   else
      wert = 1;
   return wert;
}