Revision 1899

View differences:

trunk/code/projects/traffic_navigation/highways.h
5 5
void highwayStart(void);
6 6

  
7 7
#define COUNT_MAX 12
8
#define PASS_DISTANCE 150
8
#define PASS_DISTANCE 220
9 9
#endif
trunk/code/projects/traffic_navigation/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;
trunk/code/projects/traffic_navigation/main.c
31 31
	sign = 0;
32 32
	
33 33
	//Test code
34
	state = SROAD;
34
	state = SHIGHWAY;
35 35

  
36 36
	sendBuffer[1] = id;
37

  
37
/*
38 38
	doDrive(255);
39 39
	turn(DOUBLE, ILEFT);
40
	
40
*/	
41 41
/*	while(1)
42 42
		doDrive(255);
43 43
	*/
......
98 98
			}
99 99
			break;
100 100
		case SHIGHWAY:/*On highway*/
101
			orb1_set_color(CYAN);
102
			while(!button1_click()){
101
		//	while(!button1_click()){
103 102
				highwayFSM();
104
			}
105
			state = SINTERSECTION;
103
		//	}
104
			//state = SINTERSECTION;
106 105
			break;
107 106
		default:
108 107
			usb_puts("I got stuck in an unknown state! My state is ");
......
263 262
			*/
264 263
		}
265 264
	}
266
}
265
}

Also available in: Unified diff