Jake's Humberside NXC Code Things to be done: add support for opposite directions, add unCrash() code, add sprint() code, change distance DEFs and time in sound_crash.

gistfile1.c

/*
 * This version of the drive program will only drive on the BLACK lane.
 * If WHITE turn right, if GREY turn left.
 * We assume that the only WHITE it encounters will be the inner lane.
 * This is meant as a working concept so that we have something to present.
 */

/* Time taken for 180 degree turn. */
#define time180 1000

/* A is right motor. C is the left motor. */
#define right_m OUT_A
#define left_m OUT_C

/* Pre defined distances for the distance sensor. */
#define crash_thresh 6
#define sprint_thresh 27

/* Pre defined light thresholds. Will need to be calibrated. */
#define black_grey 20
#define grey_white 40

/* If true, will attempt to recover. */
bool isCrashed = false;

/* 0 = black, 1 = grey, 2 = white */
/* 0 = Left, 1 = Forward, 2 = Right */
#define white 2
#define black 0
#define grey 1

int lastR, lastL;
int direction;

/* Returns 0 = black, 1 = grey and 2 = white when called for either the R(right) or L(left) light sensors. */
int checkLightL(){
	if(Sensor(IN_1) < black_grey){
        return black;
	}
    if(Sensor(IN_1) > black_grey && Sensor(IN_1) < grey_white){
        return grey;
    }
    if(Sensor(IN_1) > grey_white){
        return white;
    }
}
int checkLightR(){
    if(Sensor(IN_1) < black_grey){
        return black;
    }
    if(Sensor(IN_1) > black_grey && Sensor(IN_1) < grey_white){
        return grey;
    }
    if(Sensor(IN_1) > grey_white){
        return white;
    }
}

/* Is called at beginning of race to sprint ahead and orient itself. */
void sprint(){
	bool sprinting = true;
	while(sprinting){
		if(checkLightL() == black && checkLightR() == black){
			OnFwdReg(left_m, 100, OUT_REGMODE_SYNC);
			OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
			sprinting = false;
		}
		if(checkLightL() == grey && checkLightR() == grey){
			OnFwdReg(left_m, 100, OUT_REGMODE_SYNC);
			OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
			sprinting = false;
		}
		if(checkLightL() == white && checkLightR() == white){
			OnFwdReg(left_m, 100, OUT_REGMODE_SYNC);
			OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
			Until(lastL != checkLightL() || lastR != checkLightR() || IN_4 < sprint_thresh);
			if(IN_4 < sprint_thresh){
				OnFwdReg(left_m, 50, OUT_REGMODE_SYNC);
				OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
				Until(lastL != checkLightL() || lastR != checkLightR());
			}
			sprinting = false;
		}
	}
}

/* Is called when experiencing crash. */
void unCrash(){

}

/* Turns 180 degrees. */
void turn180(){
	OnFwdReg(left_m, 100, OUT_REGMODE_SYNC);
	OnFwdReg(right_m, 200, OUT_REGMODE_SYNC);
	Wait(time180);
}

void followBlack(){
    //if in black lane:
    if(checkLightL() == black && checkLightR() == black){
        OnFwdReg(left_m, 100, OUT_REGMODE_SYNC);
        OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
    }
    //if in white lane:
    if(checkLightL() == white && checkLightR() == white){
        OnFwdReg(left_m, 100, OUT_REGMODE_SYNC);
        OnFwdReg(right_m, 50, OUT_REGMODE_SYNC);
    }
    //if in grey lane:
    if(checkLightL() == grey && checkLightR() == grey){
        OnFwdReg(left_m, 50, OUT_REGMODE_SYNC);
        OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
    }
    //if in white and black lane:
    if(checkLightL() == white && checkLightR() == black){
        OnFwdReg(left_m, 100, OUT_REGMODE_SYNC);
        OnFwdReg(right_m, 50, OUT_REGMODE_SYNC);
    }
    //if in black and grey lane:
    if(checkLightL() == black && checkLightR() == grey){
        OnFwdReg(left_m, 50, OUT_REGMODE_SYNC);
        OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
    }
    //if in grey and white lane:
    if(checkLightL() == grey && checkLightR() == white){
        OnFwdReg(left_m, 50, OUT_REGMODE_SYNC);
        OnFwdReg(right_m, 100, OUT_REGMODE_SYNC);
    }
	//if opposite direction white and black:
    if(checkLightL() == black && checkLightR() == white){
		turn180();
    }
	//if opposite direction black and grey:
    if(checkLightL() == grey && checkLightR() == black){
		turn180();
    }
	//if opposite direction grey and white:
    if(checkLightL() == white && checkLightR() == grey){
		turn180();
    }
}

/* The main driving task. */
task drive_light(){
    sprint();
    while(true){
        if(isCrashed){
            unCrash();
        }
        followBlack();
        lastR = checkLightR();
        lastL = checkLightL();
    }
}

/* The task that checks for a crash. */
task sound_crash(){
    while(true){
        int i = 0;
        for(i = 0; i < 6; i++){
            if(IN_4 > crash_thresh){
                i = 0;
                break;
            }
            Wait(500);
        }
        if(i == 5){
            isCrashed = true;
        }
    }
}

/* The main task that start program and starts the drive_light and sound_crash tasks */
task main(){
    SetSensorLight(IN_1);
    SetSensorLight(IN_2);
    SetSensorSound(IN_4);
    lastR = checkLightR();
    lastL = checkLightL();
    //mandatory 5 second pause.
    Wait(5000);
    Precedes(sound, light);
}