NIBObee: Wegabfahren und gleichzeitig Hindernisserkennung

Hallo zusammen.

Ich habe ein kleines Problem und vielleicht hat jemand eine gute Idee.
Ich will für den Nibobee ein Programm schreiben (in Java) mit welchen er ein Weg verfolgen kann und gleichzeitig die Sensoren aktiv sind, dass er ein Hinderniss erkennen kann.

Ich habe lediglich die Grundausstattung des Nibobee, weswegen er zu komplexe Programme auch nicht verarbeiten kann.

Falls jemand eine gute Idee hat wäre das super.
Weiter unten findet ihr den Code mit welchem wir es versucht haben, der aber nicht funktioniert hat.

Falls nur schon jemand weiss ob es für den Nibobee überhaupt möglich ist zwei Sachen aufs Mal zu machen wäre es super wenn mir da jemand ein Tipp geben könnte

Freundliche Grüsse


************************************************** ************************************************** **********
CODE:

mport nanovm.nibobee.drivers.*;

public class Mainclass{

public static void main(String[] args) {

warte(2000);

while(true)
{
warte(2000);
gerade();
warte(2000);
links();
warte(2000);
gerade();
warte(2000);
rechts();
warte(2000);
gerade();
warte(2000);
links();
warte(2000);
gerade();
warte(2000);
rechts();
warte(2000);


}

}
static void warte(int ms){
if(Sensor.getRight() != 0)
{
Motor.setPWM(-600, -600);
Motor.setSpeed(-600, -600);
Leds.setStatus(0, true);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, false);
warte(1500);
Motor.stop();
Leds.setStatus(0, false);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, false);
warte(500);
}

if(Sensor.getLeft() != 0)
{
Motor.setPWM(-400, -600);
Motor.setSpeed(-400, -600);
Leds.setStatus(0, false);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, true);
warte(1500);
Motor.stop();
Leds.setStatus(0, false);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, false);
warte(500);}

if(Sensor.getLeft() != 0 && Sensor.getRight() !=0)
{
Motor.setPWM(-400, -500);
Motor.setSpeed(-200, -300);
Leds.setStatus(0, true);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, true);
warte(1500);
Motor.setPWM(0,0);
Motor.setSpeed(0, 0);
Leds.setStatus(0, true);
Leds.setStatus(1, true);
Leds.setStatus(2, true);
Leds.setStatus(3, true);
warte(500);
}

Clock.delayMilliseconds(ms);
if(Sensor.getRight() != 0)
{
Motor.setPWM(-600, -600);
Motor.setSpeed(-600, -600);
Leds.setStatus(0, true);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, false);
warte(1500);
Motor.stop();
Leds.setStatus(0, false);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, false);
warte(500);
}

if(Sensor.getLeft() != 0)
{
Motor.setPWM(-400, -600);
Motor.setSpeed(-400, -600);
Leds.setStatus(0, false);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, true);
warte(1500);
Motor.stop();
Leds.setStatus(0, false);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, false);
warte(500);}

if(Sensor.getLeft() != 0 && Sensor.getRight() !=0)
{
Motor.setPWM(-400, -500);
Motor.setSpeed(-200, -300);
Leds.setStatus(0, true);
Leds.setStatus(1, false);
Leds.setStatus(2, false);
Leds.setStatus(3, true);
warte(1500);
Motor.setPWM(0,0);
Motor.setSpeed(0, 0);
Leds.setStatus(0, true);
Leds.setStatus(1, true);
Leds.setStatus(2, true);
Leds.setStatus(3, true);
warte(500);
}

}

static void rechts(){
Motor.setPWM(400,200);
Motor.setSpeed(400,200);
Leds.setStatus(3, true);
Leds.setStatus(2,true);
Leds.setStatus(0, false);
Leds.setStatus(1, false);


}
static void links(){
Motor.setPWM(200,500);
Motor.setSpeed(200,400);
Leds.setStatus(0, true);
Leds.setStatus(1,true);
Leds.setStatus(3, false);
Leds.setStatus(2, false);

}
static void gerade(){
Motor.setPWM(500,635);
Motor.setSpeed(500,635);
Leds.setStatus(1, true);
Leds.setStatus(2, true);
Leds.setStatus(3, false);
Leds.setStatus(0, false);
Clock.delayMilliseconds(2000);


}
}