Help with Explorer

[west01]

Hello

I can need some help with the following program, that I made with BricxCC in NXC.

The purpose is to make a robot, wich detects with the US sensor a obstacle in front of him. Then it wil look to the left and right (the US sensor is mounted on a servo). It will see wich way is better to go (the way were no obstacle is seen) and will go that way.
I will make a random task, if it doesn't see any obstacle at the left of right. But I first I want to make the first part operational. In a later stadium I will make a bumper wich has to work also.
I borrowed the idea from http://www.nxtprograms.com/NXT2/explorer/index.html

The problem with the programm I made, is that it doesn't work as it suposed to do. When it decides to turn right: nothing is wrong. But when it turns to the left it doesn't work: it often turns right or first turn left....but immediately turns right again.
I have tried many things, but nothing (of my knowlidge) works.

Who can help me. Btw I'm a newbee here and with mindstorms/bricx/programming/......

Thanks,
David

Here is the code:

Code: Select all

int left;
int right;
int sem;

#define sem1 until (sem == 0); sem = 1;
#define sem0 sem = 0;

task move ()
{
     while (true)
     {
     sem1;
     OnFwd (OUT_AC, 50);
     sem0;
     Wait (100);
     }
}

task detect ()
{
     while (true)
     {
           if (SensorUS(IN_4) < 30)
           {
           sem1;
           Off (OUT_AC);
           RotateMotor (OUT_B, 100, 90); right = SensorUS(IN_4); Wait (1000);
           RotateMotor (OUT_B, 100, -180); left = SensorUS (IN_4); Wait (1000);
           RotateMotor (OUT_B, 100, 90);
           sem0;
           Wait (2400);

           }
     }
}

task moveleft ()
{
     while (true)
     {
           if (right < left)
           {
           sem1;
           Off (OUT_AC);
           left = 0; Wait (100);
           right = 0; Wait (100);
           OnFwd (OUT_C, 100);Wait (2000);
           sem0;
           Wait (100);
           }
     }
}

task moveright ()
{
     while (true)
     {
           if (right > left)
           {
           sem1;
           Off (OUT_AC);
           left = 0; Wait (100);
           right = 0; Wait (100);
           OnFwd (OUT_A, 100);Wait (2300);
           sem0;
           Wait (100);
           }
     }
}

task main ()
{
SetSensorUltrasonic (IN_4);
sem0;
Precedes (move, detect, moveleft, moveright);
}