Statistics
| Revision:

root / trunk / code / projects / linefollowing / lineDrive.c @ 1855

History | View | Annotate | Download (2.24 KB)

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