Ich hab in meinem Funktionsarchiv für Asuro einen Regler gefunden, den ich damals programmiert habe.
Der Code alleine ist so noch nicht lauffähig, aber alles schreib ich hier nicht hinein, denn selberdenken schadet nicht!


Code:
void fkt_geradeaus_fahren(int speed, unsigned char (*funktion)(void))
{

    unsigned char mspeed_l=0,mspeed_r=0,flag_l=0,flag_r=0;
    int y=0,e=0,odata[2]={0},counter_l=0,counter_r=0;
    unsigned long int tregeln=0;

    if(speed>0 && speed<=255)
    {
        MotorDir(FWD,FWD);
        mspeed_l=speed;
        mspeed_r=speed;
    }
    else if(speed<0 && speed>=(-255))
    {
        MotorDir(RWD,RWD);
        mspeed_l=-speed;
        mspeed_r=-speed;
    }
    else
    {
        return;
    }

    while(funktion()==0)
    {
        MotorSpeed(mspeed_l,mspeed_r);
        OdometrieData(odata);
        //Stellgröße anhand Regelabweichung berechnen

        if(odata[0]<ODOMETRIE_L && flag_l==0)
        {
            counter_l++;
            flag_l=1;
        }
        else if(odata[0]>ODOMETRIE_L && flag_l==1)
        {
            counter_l++;
            flag_l=0;
        }


        if(odata[1]<ODOMETRIE_R && flag_r==0)
        {
            counter_r++;
            flag_r=1;
        }
        else if(odata[1]>ODOMETRIE_R && flag_r==1)
        {
            counter_r++;
            flag_r=0;
        }

        if(Gettime()>tregeln)
        {
            e=counter_l-counter_r;
            y=abs(e*G_KP);

            //Stellgröße gegen Grenzwerte prüfen
            if(y>G_KORR_MAX)
                y=G_KORR_MAX;

            if(e<0)
            {
                if((int)(mspeed_r+abs(speed)-mspeed_l-y)<0)
                {
                    mspeed_r=0;
                    mspeed_l=abs(speed);
                }
                else if((int)(mspeed_r+abs(speed)-mspeed_l-y)>255)
                {
                    mspeed_r=255;
                    mspeed_l=abs(speed);
                }
                else
                {
                    mspeed_r=mspeed_r+abs(speed)-mspeed_l-y;
                    mspeed_l=abs(speed);
                }
            }

            else if(e>0)
            {
                if((int)mspeed_l+abs(speed)-mspeed_r-y<0)
                {
                    mspeed_l=0;
                    mspeed_r=abs(speed);
                }
                else if((int)mspeed_l+abs(speed)-mspeed_r-y>255)
                {
                    mspeed_l=255;
                    mspeed_r=abs(speed);
                }
                else
                {
                    mspeed_l=mspeed_l+abs(speed)-mspeed_r-y;
                    mspeed_r=abs(speed);
                }
            }

            tregeln=Gettime()+100;
            counter_l=counter_l*G_I_FAKTOR;
            counter_r=counter_r*G_I_FAKTOR;
        }

    }


    if(speed>0)
        MotorDir(RWD,RWD);
    else
        MotorDir(FWD,FWD);
    MotorSpeed(100,100);
    Msleep(100);
    MotorDir(BREAK,BREAK);
    MotorSpeed(0,0);
    return;
}