Project

General

Profile

Revision 1855

Added by Dan Jacobs over 13 years ago

lineFollow now supports merge. line drive is UNTESTED and has NO COMMENTS, but will provide higher level driving control

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