Revision 1877 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
extern int state[5];
5
static int count =0;
6
static long average = 0;
4
static int counti =0;
5
static long averagei = 0;
6
static int states = 0;
7
int canMerge = 1;
7 8

  
9
void highwayFSM(){
10
	switch(states){
11
	
12
	case 0: // Normal Drive
13
		doDrive(225);
14
		int range = range_read_distance(IR2);
15
	        usb_puti(range);
16
       		usb_puts(" : ");
17
	        usb_puti(averagei);
18
	        usb_puts("\r\n");
19
		orb1_set_color(PURPLE);
20
       		if(range == -1){
21
                	return;
22
	        }
23

  
24
        	counti ++;
25
	        averagei += range;
26
		if(counti>= COUNT_MAX){
27
			if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
28
				states = 1;
29
			averagei = 0;
30
			counti = 0;
31
		}
32
		break;
33
		case 1:
34
			merge(ILEFT);
35
			states = 2;
36
			canMerge = 0;
37
		case 2:
38
			if(doDrive(160) == FINISHED) states = 0;
39
			orb1_set_color(BLUE);
40
			break;
41
	}
42
}
43

  
44
/*
8 45
void driveHighway( void ){
9 46
	doDrive(200);
10 47
	int range = range_read_distance(IR2);
......
26 63
				usb_puts("MERGE \r\n");	
27 64
			}
28 65
		}
66
		
29 67
		count =0;
30 68
		average = 0;
31 69
	}
32 70
	//else orb1_set_color(GREEN);
33 71
}
34

  
72
*/

Also available in: Unified diff