Statistics
| Revision:

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

History | View | Annotate | Download (2.3 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)
24
        {
25
                motor_l_set(FORWARD, 0);
26
                moror_r_set(FORWARD, 0);
27
                return NORMAL;
28
        }
29

    
30

    
31
        int code;
32
        switch(state[0])
33
        {        
34
        case ISTRAIGHT:
35
                code = lineFollow(speed);
36
                if(code==INTERSECTION)
37
                {
38
                        for(int i=0; i<4; i++) state[i]=state[i+1];
39
                        state[4]=0;
40
                        if(state[0]==0)stateCounter++;
41
                        break;
42
                }
43
                else if(code==NOBARCODE) return NORMAL;
44
                return code;
45

    
46

    
47
        case ILEFT:
48
                code = turnLeft();
49
                if(code==0)
50
                {
51
                        state[0]=0;
52
                        stateCounter++;
53
                }
54
                break;
55

    
56
        case IRIGHT:
57
                code = turnRight();
58
                if(code==0)
59
                {
60
                        state[0]=0;
61
                        stateCounter++;
62
                }
63
                break;
64

    
65

    
66
        case MERGELEFT:
67
                code = mergeLeft();
68
                if(code==0)
69
                {
70
                        state[0]=0;
71
                        stateLength=0;
72
                        return FINISHED;
73
                }
74
                return NORMAL;
75

    
76
        case MERGERIGHT:
77
                code = mergeRight();
78
                if(code==0)
79
                {
80
                        state[0]=0;
81
                        stateLength=0;
82
                        return FINISHED;
83
                }
84
                return NORMAL;
85

    
86
        default:
87
                return LOST;
88

    
89
        }
90

    
91
        if(stateCounter>=stateLength)
92
        {
93
                stateCounter=stateLength=0;
94
                return FINISHED;
95
        }
96
        return NORMAL;
97
}
98

    
99

    
100
void start(void){stopped=0;}
101
void stop(void){stopped=1;}
102

    
103

    
104
int merge(int dir)
105
{
106
        if(stateLength!=0)return ERROR;
107
        stateLength++;
108
        state[0]=(dir==ILEFT ? MERGELEFT : MERGERIGHT);
109
        return NORMAL;
110
}
111

    
112
int turn(int type, int dir)
113
{
114
        if(stateLength!=0)return ERROR;
115
        if(dir==IRIGHT){stateLength++; state[1]=IRIGHT; return NORMAL;}
116
        if(dir==IUTURN){stateLength+=2;  state[1]=state[2]=ILEFT; return NORMAL;}
117
        if(dir==ISTRAIGHT && type==SINGLE){stateLength++; state[1]=ISTRAIGHT; return NORMAL;}
118
        if(dir==ISTRAIGHT){stateLength+=2; state[1]=state[2]=ISTRAIGHT; return NORMAL;}
119
        //must be left turn
120
        if(type==SINGLE){stateLength++; state[1]=ILEFT; return NORMAL;}
121
        if(type==DOUBLE){stateLength+=3;state[1]=state[3]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
122
        if(type==ON_RAMP){stateLength+=2; state[1]=ILEFT; state[2]=ISTRAIGHT; return NORMAL;}
123
        if(type==OFF_RAMP){stateLength+=2; state[1]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
124

    
125
        //Should never get here
126
        return ERROR;
127
}
128