Statistics
| Revision:

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

History | View | Annotate | Download (3.83 KB)

1
/**
2
 * @file lineDrive.c
3
 *
4
 * Provides functions to implement line driving behavior.  This program extends
5
 * the behavior of the line-following program by following lines automatically
6
 * and implementing behaviors to deal with commands passed to lineDrive.
7
 *
8
 * @author Dan Jacobs
9
 * @date 11-1-2010
10
 */
11

    
12
#include "lineDrive.h"
13

    
14
int state[5];       //! Stores a queue of sub-commands to be executed
15
int stateCounter;
16
int stateLength;
17

    
18
//! Whether lineDrive is currently paused. Set to 0 on initialization.
19
int stopped=1;
20

    
21

    
22
/**
23
 * Starts the line following procedure. Must be called before other
24
 * line-following functions will work.  This function essentially resets the
25
 * state of line-following.
26
 */
27
void lineDrive_init()
28
{
29
        lineFollow_init();
30
        for(int i=0; i<5; i++)state[i]=0;
31
        stateCounter=0;
32
        stateLength=0;
33
        stopped=0;
34
}
35

    
36

    
37
/**
38
 * Follows a line and executes whatever command is next on the queue.
39
 * @param speed The speed with which to drive along the line.
40
 */
41
int doDrive(int speed)
42
{
43
        if(stopped)
44
        {
45
                motor_l_set(FORWARD, 0);
46
                motor_r_set(FORWARD, 0);
47
                return NORMAL;
48
        }
49

    
50

    
51
        int code;
52
        switch(state[0])
53
        {        
54
        case ISTRAIGHT:
55
                code = lineFollow(speed);
56
                if(code==INTERSECTION)
57
                {
58
                        for(int i=0; i<4; i++) state[i]=state[i+1];
59
                        state[4]=0;
60
                        if(state[0]==0)stateCounter++;
61
                        break;
62
                }
63
                else if(code==NOBARCODE) return NORMAL;
64
                return code;
65

    
66

    
67
        case ILEFT:
68
                code = turnLeft();
69
                if(code==0)
70
                {
71
                        state[0]=0;
72
                        stateCounter++;
73
                }
74
                break;
75

    
76
        case IRIGHT:
77
                code = turnRight();
78
                if(code==0)
79
                {
80
                        state[0]=0;
81
                        stateCounter++;
82
                }
83
                break;
84

    
85

    
86
        case MERGELEFT:
87
                code = mergeLeft();
88
                if(code==0)
89
                {
90
                        state[0]=0;
91
                        stateLength=0;
92
                        return FINISHED;
93
                }
94
                return NORMAL;
95

    
96
        case MERGERIGHT:
97
                code = mergeRight();
98
                if(code==0)
99
                {
100
                        state[0]=0;
101
                        stateLength=0;
102
                        return FINISHED;
103
                }
104
                return NORMAL;
105

    
106
        default:
107
                return LOST;
108

    
109
        }
110

    
111
        if(stateCounter>=stateLength)
112
        {
113
                stateCounter=stateLength=0;
114
                return FINISHED;
115
        }
116
        return NORMAL;
117
}
118

    
119

    
120
/** Starts the line-drive process if paused. */
121
void start(void){stopped=0;}
122

    
123
/** Pauses the line-drive process. Default is started. */
124
void stop(void){stopped=1;}
125

    
126

    
127
/**
128
 * Defines a merge command in the direction specified.  A merge is a switch
129
 * of lanes.
130
 * @param dir Left or right, defined by ILEFT or IRIGHT
131
 */
132
int merge(int dir)
133
{
134
        if(stateLength!=0)return ERROR;
135
        stateLength++;
136
        state[0]=(dir==ILEFT ? MERGELEFT : MERGERIGHT);
137
        return NORMAL;
138
}
139

    
140
/**
141
 * Executes an intersection turn where the intersection type is specified by the
142
 * parameters. 
143
 * @param type A valid defined intersection type
144
 * @param dir The direction to turn at the intersection
145
 */
146
int turn(int type, int dir)
147
{
148
        if(stateLength!=0)return ERROR;
149
        if(dir==IRIGHT)
150
    {
151
        stateLength++;
152
        state[1]=IRIGHT;
153
        return NORMAL;
154
    }
155
        if(dir==IUTURN)
156
    {
157
        stateLength+=2;
158
        state[1]=state[2]=ILEFT;
159
        return NORMAL;
160
    }
161
        if(dir==ISTRAIGHT && type==SINGLE)
162
    {
163
        stateLength++;
164
        state[1]=ISTRAIGHT;
165
        return NORMAL;
166
    }
167
        if(dir==ISTRAIGHT)
168
    {
169
        stateLength+=2;
170
        state[1]=state[2]=ISTRAIGHT;
171
        return NORMAL;
172
    }
173
        // At this point, must be left turn
174
        if(type==SINGLE)
175
    {
176
        stateLength++;
177
        state[1]=ILEFT;
178
        return NORMAL;
179
    }
180
        if(type==DOUBLE_C || type==DOUBLE_T)
181
    {
182
        stateLength+=3;
183
        state[1]=state[3]=ISTRAIGHT;
184
        state[2]=ILEFT;
185
        return NORMAL;
186
    }
187
        if(type==ON_RAMP)
188
    {
189
        stateLength+=2;
190
        state[1]=ILEFT;
191
        state[2]=ISTRAIGHT;
192
        return NORMAL;
193
    }
194
        if(type==OFF_RAMP)
195
    {
196
        stateLength+=2;
197
        state[1]=ISTRAIGHT;
198
        state[2]=ILEFT;
199
        return NORMAL;
200
    }
201

    
202
        //Should never get here
203
        return ERROR;
204
}
205