Revision 1890

View differences:

trunk/code/projects/traffic_navigation/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
/*
trunk/code/projects/traffic_navigation/main.c
26 26
	sign = 0;
27 27
	
28 28
	//Test code
29
	state = SROAD;
29
	state = SHIGHWAY;
30 30

  
31 31
	sendBuffer[1] = id;
32

  
32
/*
33 33
	doDrive(255);
34 34
	turn(DOUBLE, ILEFT);
35 35
	
36 36
	while(1)
37 37
		doDrive(255);
38 38
	
39
	
39
*/	
40 40
	while (1) {
41 41
		/*DTM Finite State Machine*/
42 42
		switch(state){
......
88 88
			}
89 89
			break;
90 90
		case SHIGHWAY:/*On highway*/
91
		//	usb_puts("HIGHWAY\r\n");
92
			if(button1_click()) highwayStart();
93
			highwayFSM();
91 94
//			sign = lineFollow();
92 95
			//highway behaviors
93 96
				//merging
94 97
				//passing
95 98
			//read barcode
96 99
			break;
97
			break;
98 100
		default:
99 101
			usb_puts("I got stuck in an unknown state! My state is ");
100 102
			usb_puti(state);
......
249 251
			*/
250 252
		}
251 253
	}
252
}
254
}
trunk/code/projects/traffic_navigation/highways.h
1 1
#ifndef HIGHWAYS_H
2 2
#define HIGHWAYS_H
3 3

  
4
void highwayFSM(void);
4
int highwayFSM(void);
5
void highwayStart(void);
5 6

  
6

  
7 7
#define COUNT_MAX 12
8 8
#define PASS_DISTANCE 150
9 9
#endif

Also available in: Unified diff