Revision 1855

View differences:

trunk/code/projects/linefollowing/lineDrive.c
1
#include "lineDrive.h"
2

  
3
int state[5];
4
int stateCounter;
5
int stateLength;
6
int stopped=1;
7

  
8

  
9

  
10
void lineDrive_init()
11
{
12
	lineFollow_init();
13
	for(int i=0; i<5; i++)state[i]=0;
14
	stateCounter=0;
15
	stateLength=0;
16
	stopped=0;
17
}
18

  
19

  
20

  
21
int doDrive(int speed)
22
{
23
	if(stopped)return NORMAL;
24

  
25
	int code;
26
	switch(state[0])
27
	{	
28
	case ISTRAIGHT:
29
		code = lineFollow(speed);
30
		if(code==INTERSECTION)
31
		{
32
			for(int i=0; i<4; i++) state[i]=state[i+1];
33
			state[4]=0;
34
			if(state[0]==0)stateCounter++;
35
			break;
36
		}
37
		else if(code==NOBARCODE) return NORMAL;
38
		return code;
39

  
40

  
41
	case ILEFT:
42
		code = turnLeft();
43
		if(code==0)
44
		{
45
			state[0]=0;
46
			stateCounter++;
47
		}
48
		break;
49

  
50
        case IRIGHT:
51
                code = turnRight();
52
                if(code==0)
53
                {
54
			state[0]=0;
55
			stateCounter++;
56
		}
57
                break;
58

  
59

  
60
	case MERGELEFT:
61
		code = mergeLeft();
62
		if(code==0)
63
		{
64
			state[0]=0;
65
			stateLength=0;
66
			return FINISHED;
67
		}
68
		return NORMAL;
69

  
70
	case MERGERIGHT:
71
                code = mergeRight();
72
                if(code==0)
73
                {
74
                        state[0]=0;
75
                        stateLength=0;
76
			return FINISHED;
77
                }
78
                return NORMAL;
79

  
80
	default:
81
		return LOST;
82

  
83
	}
84

  
85
	if(stateCounter>=stateLength)
86
	{
87
		stateCounter=stateLength=0;
88
		return FINISHED;
89
	}
90
	return NORMAL;
91
}
92

  
93

  
94
void start(void){stopped=0;}
95
void stop(void){stopped=1;}
96

  
97

  
98
int merge(int dir)
99
{
100
	if(stateLength!=0)return ERROR;
101
	stateLength++;
102
	state[0]=(dir==ILEFT ? MERGELEFT : MERGERIGHT);
103
	return NORMAL;
104
}
105

  
106
int turn(int type, int dir)
107
{
108
	if(stateLength!=0)return ERROR;
109
	if(dir==IRIGHT){stateLength++; state[1]=IRIGHT; return NORMAL;}
110
	if(dir==IUTURN){stateLength+=2;  state[1]=state[2]=ILEFT; return NORMAL;}
111
	if(dir==ISTRAIGHT && type==SINGLE){stateLength++; state[1]=ISTRAIGHT; return NORMAL;}
112
	if(dir==ISTRAIGHT){stateLength+=2; state[1]=state[2]=ISTRAIGHT; return NORMAL;}
113
	//must be left turn
114
	if(type==SINGLE){stateLength++; state[1]=ILEFT; return NORMAL;}
115
	if(type==DOUBLE){stateLength+=3;state[1]=state[3]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
116
	if(type==ON_RAMP){stateLength+=2; state[1]=ILEFT; state[2]=ISTRAIGHT; return NORMAL;}
117
	if(type==OFF_RAMP){stateLength+=2; state[1]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
118

  
119
	//Should never get here
120
	return ERROR;
121
}
122

  
trunk/code/projects/linefollowing/lineDrive.h
1
#ifndef _LINE_DRIVE_
2
#define _LINE_DRIVE_
3

  
4
#include "lineFollow.h"
5

  
6
#define DOUBLE		0
7
#define SINGLE		1
8
#define ON_RAMP		2
9
#define OFF_RAMP	3
10

  
11
#define ISTRAIGHT	0
12
#define ILEFT		1
13
#define IRIGHT		2
14
#define IUTURN		3
15

  
16
#define MERGELEFT	4
17
#define MERGERIGHT	5
18

  
19
#define NORMAL		-1
20
#define FINISHED	-2
21
#define LOST		-3
22
#define ERROR		-4
23

  
24

  
25

  
26
void lineDrive_init(void);
27

  
28

  
29

  
30
int doDrive(int speed);
31

  
32
void start(void);
33
void stop(void);
34

  
35

  
36
int merge(int dir);
37

  
38
int turn(int type, int dir);
39

  
40
#endif

Also available in: Unified diff