Revision 1899 trunk/code/projects/traffic_navigation/highways.c

View differences:

highways.c
6 6
static long averagei = 0;
7 7
static int states = 0;
8 8
int canMerge = 1;
9

  
9 10
void highwayStart(){
10 11
	canMerge = 1;
11 12
}
13
int changeLanes(){
14
        if(counti<10000){
15
		counti++;
16
		motor_r_set(FORWARD,232);
17
		motor_l_set(FORWARD,200);
18
		return 0;
19
        }
20
	int colours[5];
21
	updateLine(colours);
22
	int pos = lineLocate(colours);
23
	if(pos >= 3 || pos <= -3){
24
		motor_r_set(FORWARD,187);
25
		motor_l_set(FORWARD,190);
26
		return 0;
27
	}
28
	counti = 0;
29
	return 1;
30
}
12 31
int highwayFSM(){
13 32
	switch(states){
14 33
	
15 34
	case 0: // Normal Drive
16 35
		{
17
		int theCode = doDrive(225);
36
		int theCode = doDrive(205);
18 37
/*		if(theCode != NORMAL && theCode >= 0)
19 38
			return theCode;
20 39
*/
......
23 42
       		usb_puts(" : ");
24 43
	        usb_puti(averagei);
25 44
	        usb_puts("\r\n");
26
		orb1_set_color(PURPLE);
45
		orb1_set_color(CYAN);
27 46
       		if(range == -1){
28 47
                	range = 200;
29 48
	        }
......
38 57
		break;
39 58
		}
40 59
	case 1:
41
			merge(ILEFT);
60
			//merge(ILEFT);
42 61
			states = 2;
43 62
			canMerge = 0;
44 63
	case 2:
45
			if(doDrive(160) == FINISHED) states = 0;
46
			orb1_set_color(BLUE);
64
			if(changeLanes()){
65
			   states = 0;
66
			   orb1_set_color(GREEN);
67
			}
68
			else orb1_set_color(YELLOW);
47 69
			break;
48 70
	}
49 71
	return 0;

Also available in: Unified diff