Bumper / ACS Fehlfunktion

[Dasive]

Miit meinem Programm, dachte da sei die Fehlerchance gering:

#include "RP6RobotBaseLib.h"

void RobotBase_initialisieren (void){
  initRobotBase();
  writeString_P("RobotBase initialisiert\n");
}

void Motorkomponenten_starten (void){
  powerON();
  writeString_P("Energie fuer Motorenkomponenten freigegeben\n");
}

void LEDs_pruefen(void){
  int LEDs_angeschaltet = 0;
  while(LEDs_angeschaltet < 3)
  {
    statusLEDs.LED1 = true;
    statusLEDs.LED4 = true;
    updateStatusLEDs();
    mSleep(100);
    statusLEDs.LED2 = true;
    statusLEDs.LED5 = true;
    updateStatusLEDs();
    mSleep(100);
    statusLEDs.LED3 = true;
    statusLEDs.LED6 = true;
    updateStatusLEDs();
    mSleep(100);
  
    statusLEDs.LED1 = false;
    statusLEDs.LED4 = false;
    updateStatusLEDs();
    mSleep(100);
    statusLEDs.LED2 = false;
    statusLEDs.LED5 = false;
    updateStatusLEDs();
    mSleep(100);
    statusLEDs.LED3 = false;
    statusLEDs.LED6 = false;
    updateStatusLEDs();
    mSleep(100);
	LEDs_angeschaltet = LEDs_angeschaltet +1;
  }
  
  int Alle_LEDs_angeschaltet = 0;
  while(Alle_LEDs_angeschaltet < 3)
  {
    statusLEDs.LED1 = true;
	statusLEDs.LED2 = true;
	statusLEDs.LED3 = true;
	statusLEDs.LED4 = true;
	statusLEDs.LED5 = true;
	statusLEDs.LED6 = true;
	updateStatusLEDs();
	mSleep(100);
	
	statusLEDs.LED1 = false;
	statusLEDs.LED2 = false;
	statusLEDs.LED3 = false;
	statusLEDs.LED4 = false;
	statusLEDs.LED5 = false;
	statusLEDs.LED6 = false;
	updateStatusLEDs();
	mSleep(100);
	Alle_LEDs_angeschaltet = Alle_LEDs_angeschaltet +1;
  }
  writeString_P("LEDs geprueft\n");
}

void LED_Zustaende_ausgeben(void){
  if(statusLEDs.LED1)
    writeString_P(" LED 1: An, ");
  else
	writeString_P(" LED 1: Aus, ");
	
  if(statusLEDs.LED2)
    writeString_P("LED 2: An, ");
  else
	writeString_P("LED 2: Aus, ");
	
  if(statusLEDs.LED3)
    writeString_P("LED 3: An, ");
  else
	writeString_P("LED 3: Aus, ");
	
  if(statusLEDs.LED4)
    writeString_P("LED 4: An, ");
  else
	writeString_P("LED 4: Aus, ");
	
  if(statusLEDs.LED5)
    writeString_P("LED 5: An, ");
  else
	writeString_P("LED 5: Aus, ");
	
  if(statusLEDs.LED6)
    writeString_P("LED 6: An\n");
  else
	writeString_P("LED 6: Aus\n");
}
void Bumper_Zustaende_ausgeben(void){
  task_Bumpers();
  if(bumper_left)
    writeString_P(" Linker Bumper: aktiv ,");
  else
    writeString_P(" Linker Bumper: inaktiv ,");
	
  if(bumper_right)
    writeString_P("Rechter Bumper: aktiv\n");
  else
    writeString_P("Rechter Bumper: inaktiv\n");
}
void ACS_Zustaende_ausgeben(void){
  setACSPwrLow();
  task_ACS();
  if (obstacle_left == true && obstacle_right == true)
    writeString_P(" Hinderniss nahe vorne:   ja, ");
  else
    writeString_P(" Hinderniss nahe vorne:   nein, ");
	
  if(obstacle_left == true)
    writeString_P("Hinderniss nahe links:   ja, ");
  else
    writeString_P("Hinderniss nahe links:   nein, ");
	
  if(obstacle_right == true)
    writeString_P("Hinderniss nahe rechts:   ja\n");
  else
    writeString_P("Hinderniss nahe rechts:   nein\n");
	
  mSleep(50);	
  setACSPwrMed();
  task_ACS();
  if (obstacle_left == true && obstacle_right == true)
    writeString_P(" Hinderniss mittel vorne: ja, ");
  else
    writeString_P(" Hinderniss mittel vorne: nein, ");
	
  if(obstacle_left == true)
    writeString_P("Hinderniss mittel links: ja, ");
  else
    writeString_P("Hinderniss mittel links: nein, ");
	
  if(obstacle_right == true)
    writeString_P("Hinderniss mittel rechts: ja\n");
  else
    writeString_P("Hinderniss mittel rechts: nein\n");
	
  mSleep(50);	
  setACSPwrHigh();
  task_ACS();
  if (obstacle_left == true && obstacle_right == true)
    writeString_P(" Hinderniss weit vorne:   ja, ");
  else
    writeString_P(" Hinderniss weit vorne:   nein, ");
	
  if(obstacle_left == true)
    writeString_P("Hinderniss weit links:   ja, ");
  else
    writeString_P("Hinderniss weit links:   nein, ");
	
  if(obstacle_right == true)
    writeString_P("Hinderniss weit rechts:   ja\n");
  else
    writeString_P("Hinderniss weit rechts:   nein\n");
	
  mSleep(50);	
  setACSPwrOff();
}
void Zustaende_ausgeben(void){
  writeString_P("Zustaende:\n");
  LED_Zustaende_ausgeben();
  Bumper_Zustaende_ausgeben();
  ACS_Zustaende_ausgeben();
}

void Fahre_Rueckwaerts(int Distanz)
{
  
}
void Bumper_beruehrt(void) {
    if (bumper_left == true && bumper_right == true)
	{
	}
	else if(bumper_left)
	{
	}
	else
	{
	}
} 
int main (void)
{
  RobotBase_initialisieren();
  Motorkomponenten_starten();
  LEDs_pruefen();
  Zustaende_ausgeben();
  BUMPERS_setStateChangedHandler(Bumper_beruehrt); 
  
  moveAtSpeed(70,70); 
   
  while(true)
  {
    task_RP6System();
  }
  
  return 0;
}