Revision 1877

View differences:

trunk/code/projects/traffic_navigation/traffic_navigation.h
3 3
#include <dragonfly_lib.h>
4 4
#include <wl_basic.h>
5 5
#include "lineDrive.h"
6
#include "highways.h"
6 7

  
7 8
/*States*/
8 9
#define SROAD 0
......
56 57
 */
57 58
#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE)
58 59

  
59
#endif
60
#endif
trunk/code/projects/traffic_navigation/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
*/
trunk/code/projects/traffic_navigation/main.c
208 208
			}
209 209
			break;
210 210
		case SHIGHWAY:/*On highway*/
211
			driveHighway();
212
			break;
211
			highwayFSM();
213 212
		default:
214 213
			usb_puts("I got stuck in an unknown state! My state is ");
215 214
			usb_puti(state);
trunk/code/projects/traffic_navigation/highways.h
4 4
void driveHighway(void);
5 5

  
6 6

  
7
#define COUNT_MAX 5
8
#define PASS_DISTANCE 100
7
#define COUNT_MAX 8
8
#define PASS_DISTANCE 150
9 9
#endif

Also available in: Unified diff