lineFollower.nxc

#define THRESHOLD 40

unsigned char tmp[4]={2,2,2,2};
unsigned char x=3;
task main()
{
    unsigned char tmp_x;
    unsigned char tmp_i;
    SetSensorLight(IN_1);
    SetSensorLight(IN_2);
    SetSensorLight(IN_3);
    while(true) {
        x=(x+1)&3;
        tmp[x] = (((Sensor(IN_1) <= THRESHOLD)?1:0)|
                ((Sensor(IN_2) <= THRESHOLD)?2:0)|
                ((Sensor(IN_3) <= THRESHOLD)?4:0));
        switch(tmp[x] ){
            case 4: //hard right
                OnFwd(OUT_B, 30);
                OnRev(OUT_A, 30);

            break;
            case 1: //hard left
                OnFwd(OUT_A, 30);
                OnRev(OUT_B, 30);

            break;
            case 6: //light left
                OnFwd(OUT_A, 20);
                OnFwd(OUT_B, 40);

            break;
            case 3: //light right
                OnFwd(OUT_A, 40);
                OnFwd(OUT_B, 20);

            break;
            case 2: /*nothing :D*/
                OnFwd(OUT_AB, 45);

            break;

            case 0: //all 0
            case 5: //middle nothing
            case 7: //all 1
                for(tmp_i=1;tmp_i<4;tmp_i++){
                    
                    tmp_x = tmp[(x - tmp_i) & 3];
                    if(!(tmp_x == 0 || tmp_x == 5 || tmp_x == 7)){
                        tmp_i = 0;
                        break;
                    }
                }
                if(tmp_i){
                    if(tmp_x == 0){
                        //no line
                        OnFwd(OUT_AB, 25);
                    }
                    else{
                        //Problem
                        Off(OUT_AB);
                    }
                }
            break;
        }
        Wait(25);
    }
}