task_ACS(); oder setACSPwr...(); tut nicht

[Luki.B.]

Hi,
ich wolte einen ACS-Eventhandler schreiben welcher versucht den Hindernissen auszuweichen und die Entfernung zum Hinderniss berücksichtigt und dementsprechend enge Kurven macht indem die ACS-Leistung laufend geändert wird um festzustellen wie nahe das Honderniss ist.

enum direction {Right, Left, Forwd, Backwd}

void zuruecksetzendrehen (enum direction dir)
{
    setLEDs(0b100000);
    move (50, BWD, DIST_MM(150), false);
    while (!isMovementComplete())
    {
        task_motionControl();
    }
    
    if (dir==Right)
    {
        rotate (40, RIGHT, 90, false);
        while (!isMovementComplete())
        {
            task_motionControl();
        }
    };
    
    if (dir==Left)
    {
        rotate (40, LEFT, 90, false);
        while (!isMovementComplete())
        {
            task_motionControl();
        }  
    }
    
    changeDirection(FWD);
}

void ACSHandler_UmfahrenNahe(void)
{
    setLEDs(0b111111);
        setACSPwrLow();
        task_ACS();
        mSleep(100);
         while (obstacle_left)
         {
             if (obstacle_right && obstacle_left)
             {
                 zuruecksetzendrehen(Right);
                 setLEDs(0b111111);
             }

             task_ACS();
             mSleep(100);
             moveAtSpeed(100,30);
             task_motionControl();
         }
        
        while (obstacle_right)
        {
             if (obstacle_right && obstacle_left)
             {
                 zuruecksetzendrehen(Right);
                 setLEDs(0b011011);
             }

             task_ACS();
             mSleep(100);
             moveAtSpeed(30,100);
             task_motionControl();
        }
    setLEDs(0b111111);
}

void ACSHandler_UmfahrenMittel (void)
{
    setLEDs(0b011011);
    setACSPwrMed();
        task_RP6System();
        mSleep(100);
         while (obstacle_left)
         {
             if (obstacle_right && obstacle_left)
             {
                 zuruecksetzendrehen(Right);
                 setLEDs(0b011011);
             }
             
             setACSPwrLow();
             task_ACS();
             mSleep(100);
             if (obstacle_left || obstacle_right)
                 ACSHandler_UmfahrenNahe();
             
             setACSPwrMed();
             task_ACS();
             mSleep(100);
             moveAtSpeed(100,50);
             task_motionControl();
         }
        
        while (obstacle_right)
        {
             if (obstacle_right && obstacle_left)
             {
                 zuruecksetzendrehen(Right);
                 setLEDs(0b011011);
             }
             
             setACSPwrLow();
             task_ACS();
             mSleep(100);
             if (obstacle_left || obstacle_right)
                 ACSHandler_UmfahrenNahe();
             
             setACSPwrMed();
             task_ACS();
             mSleep(100);
             moveAtSpeed(50,100);
             task_motionControl();
        }
    setLEDs(0b001001);
}

void ACSHandler_UmfahrenWeit(void)
{
    setLEDs(0b001001);
    task_ACS();
    mSleep(100);
    while (obstacle_left)
    {
        if (obstacle_right)
        {
            zuruecksetzendrehen(Right);
            setLEDs(0b001001);
        }
             
        setACSPwrMed();
        mSleep(100);
        task_ACS();
        mSleep(100);
        if (obstacle_left || obstacle_right)
            ACSHandler_UmfahrenMittel();
             
        setACSPwrHigh();
        moveAtSpeed(100,70);
        task_motionControl();
        task_ACS();
        mSleep(100);
    }
        
      while (obstacle_right)
      {
            if (obstacle_right && obstacle_left)
             {
                 zuruecksetzendrehen(Right);
                 setLEDs(0b001001);
             }
             
             setACSPwrMed();
             mSleep(100);
             task_ACS();
             mSleep(100);
             if (obstacle_left || obstacle_right)
                 ACSHandler_UmfahrenMittel();
             
             setACSPwrHigh();
             moveAtSpeed(70,100);
             task_motionControl();
             task_ACS();
             mSleep(100);
        }
    setLEDs(0b000000);
}

ich habe nach jedem Aufruf von task_ACS() mSleep(100) aufgerufen um der Funktion Zeit zu geben sich abzuarbeiten um zu verhindern dass die Funktion noch garnicht fertig ist wenn obstacle_right oder obstacle_left abzufragen.

Der Roboter geht aber sofort nach dem Aufruf von ACSHandler_UmfahrenWeit() in ACSHandler_UmfahrenMittel() und sofort danach in ACSHandler_UmfahrenNahe() wie die LEDs mir sagen.

Kann es sein dass ich auch den Funktionen setACSPwr...() Zeit geben muss bevor ich task_ACS() aufrufen kann?

Gruß Jan Lukas