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

View differences:

highways.c
1 1
#include "highways.h"
2 2
#include "lineDrive.h"
3 3
#include <dragonfly_lib.h>
4

  
4 5
static int counti =0;
5 6
static long averagei = 0;
6 7
static int states = 0;
7 8
int canMerge = 1;
8

  
9
void highwayFSM(){
9
void highwayStart(){
10
	canMerge = 1;
11
}
12
int highwayFSM(){
10 13
	switch(states){
11 14
	
12 15
	case 0: // Normal Drive
13
		doDrive(225);
14
		int range = range_read_distance(IR3);
16
		{
17
		int theCode = doDrive(225);
18
		if(theCode != NORMAL && theCode >= 0)
19
			return theCode;
20
		int range = range_read_distance(IR2);
15 21
	        usb_puti(range);
16 22
       		usb_puts(" : ");
17 23
	        usb_puti(averagei);
18 24
	        usb_puts("\r\n");
19 25
		orb1_set_color(PURPLE);
20 26
       		if(range == -1){
21
                	return;
27
                	range = 200;
22 28
	        }
23 29
        	counti ++;
24 30
	        averagei += range;
......
29 35
			counti = 0;
30 36
		}
31 37
		break;
32
		case 1:
38
		}
39
	case 1:
33 40
			merge(ILEFT);
34 41
			states = 2;
35 42
			canMerge = 0;
36
		case 2:
43
	case 2:
37 44
			if(doDrive(160) == FINISHED) states = 0;
38 45
			orb1_set_color(BLUE);
39 46
			break;
40 47
	}
48
	return 0;
41 49
}
42 50

  
43 51
/*

Also available in: Unified diff