Project

General

Profile

Revision 1931

Added by Dan Jacobs about 13 years ago

fixed ??? trunk

View differences:

trunk/code/projects/linefollowing/lineFollow.h
1 1
#include <dragonfly_lib.h>
2 2

  
3
 #ifndef _LINEFOLLOW_H_
4
 #define _LINEFOLLOW_H_
3
#ifndef _LINEFOLLOW_H_
4
#define _LINEFOLLOW_H_
5 5

  
6 6
#define LWHITE			0
7 7
#define LGREY			1
8 8
#define LBLACK	 		2
9 9
#define CENTER			3
10
#define NOLINE			-42
11 10
#define LINELOST		-1
12 11

  
13 12
#define NOBARCODE 		-2
14
#define INTERSECTION 		-25
15
#define FULL_LINE 		-26
13
#define INTERSECTION 	-25
16 14

  
15
#define NOLINE			-50
16
#define FULL_LINE 		-51
17 17

  
18

  
19
/* 	lineFollow_init
20
	Must call before lineFollow
21
	Turns analog loop off
22
*/ 
18
/**
19
 * @brief Initializes line following.
20
 * 
21
 * Must be called before line following will work.
22
 * Turns the analog loop off.
23
 */
23 24
void lineFollow_init(void);
24 25

  
25
/*	lineFollow
26
	Must call lineFollow first
27
	Must be called inside a loop
28
*/
26
/**	lineFollow
27
 *	Must call lineFollow_init first
28
 *	Must be called inside a loop
29
 */
29 30
int lineFollow(int speed);
30 31

  
31
/*	turnLeft turnRight mergeLeft mergeRight
32
	Must be called inside a loop
33
	returns 0 when complete
34
*/
32
/**	turnLeft turnRight mergeLeft mergeRight
33
 *	Must be called inside a loop
34
 *	returns 0 when complete
35
 */
35 36
int turnLeft(void);
36 37
int turnRight(void);
37 38
int mergeLeft(void);
38 39
int mergeRight(void);
39 40

  
40
/*	updateLine
41
	Reads in the analog values
42
	Fills the given array with WHITE
43
	or BLACK representing the line
44
*/
41
/**
42
 * @brief Updates the values stored in the array to white or black based on
43
 * current sensor readings.
44
 *
45
 * @param values The array of five integers to be updated. 
46
 */
45 47
void updateLine(int* values); 
46 48

  
47
/*	lineLocate
48
	Finds the location of the line
49
	Outputs positive for right side
50
	Negative for left, or NOLINE if a line is not found
51
*/
49
/**
50
 * @brief Returns an index of the middle of the line based on line readings.
51
 *
52
 * Two special return values are possible:
53
 *   NOLINE if none of the sensors holds a black value, and
54
 *   FULL_LINE if all of the sensors see black.
55
 *
56
 * Otherwise, returns a value from -4 (farthest left) to 4 (farthest right), with
57
 * 0 the line being centered in the middle.
58
 *
59
 * @param colors The array of 5 readings from the line sensor.  Must be either
60
 *    LWHITE or LBLACK.
61
 * @return Either a special value or an index from -4 to 4.
62
 *
63
 */
52 64
int lineLocate(int* colors);
53 65

  
54
/*	updatebarCode
55
	Reads in and processes
56
	bar code data
57
*/
66
/**	updatebarCode
67
 *	Reads in and processes
68
 *	bar code data
69
 */
58 70
void updateBarCode(void);
59 71

  
60
/*	getBarCode
61
	returns a bar code, if
62
	available, otherwise NOBARCODE
63
*/
72
/**
73
 * @brief Gets the completed value read by the barcode reader, or NOBARCODE.
74
 *
75
 * Returns a bar code if available (if at the end of a barcode) and resets the
76
 * barcodePosition to 0. Otherwise, return NOBARCODE.  *
77
 * @return The value of the barcode if a complete barcode, else NOBARCODE.
78
 */
64 79
int getBarCode(void);
65 80

  
66
/*	min max
67
	returns the minimum/maximum of two values
68
*/
81

  
82
//! A simple function to return the minimum of two integers.
69 83
int min(int x, int y);
84
//! A simple function to return the maximum of two integers.
70 85
int max(int x, int y);
71 86

  
72
/*	motorLeft
73
	Commands the left motor
74
	Cannot be used to stop
75
	0-126 are backward
76
	127-255 are forward
77
*/
87
/** @todo Alex: I hate these functions, but I'm keeping them so code will still work. But we should delete them sometime. */
88

  
89
/**	motorLeft
90
 *	Commands the left motor
91
 *	Cannot be used to stop
92
 *	0-126 are backward
93
 *	127-255 are forward
94
 */
78 95
void motorLeft(int speed);
79 96

  
80
/*	motorRight
81
	Commands the right motor
82
	Cannot be used to stop
83
	0-126 are backward
84
	127-255 are forward
85
*/
97
/**	motorRight
98
 *	Commands the right motor
99
 *	Cannot be used to stop
100
 *	0-126 are backward
101
 *	127-255 are forward
102
 */
86 103
void motorRight(int speed);
87 104

  
88
/*	lost
89
	Internal counter to detect if the line was lost
90
*/
105
/**	lost
106
 *	Internal counter to detect if the line was lost
107
 */
91 108
int lost;
92 109

  
110
int onLine(void);
111

  
93 112
#endif
trunk/code/projects/linefollowing/lineDrive.c
146 146
int turn(int type, int dir)
147 147
{
148 148
	if(stateLength!=0)return ERROR;
149
	if(dir==IRIGHT){stateLength++; state[1]=IRIGHT; return NORMAL;}
150
	if(dir==IUTURN){stateLength+=2;  state[1]=state[2]=ILEFT; return NORMAL;}
151
	if(dir==ISTRAIGHT && type==SINGLE){stateLength++; state[1]=ISTRAIGHT; return NORMAL;}
152
	if(dir==ISTRAIGHT){stateLength+=2; state[1]=state[2]=ISTRAIGHT; return NORMAL;}
153
	//must be left turn
154
	if(type==SINGLE){stateLength++; state[1]=ILEFT; return NORMAL;}
155
	if(type==DOUBLE){stateLength+=3;state[1]=state[3]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
156
	if(type==ON_RAMP){stateLength+=2; state[1]=ILEFT; state[2]=ISTRAIGHT; return NORMAL;}
157
	if(type==OFF_RAMP){stateLength+=2; state[1]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
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
    }
158 201

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

  
trunk/code/projects/linefollowing/lineDrive.h
3 3

  
4 4
#include "lineFollow.h"
5 5

  
6
#define DOUBLE		0
6
/* Old definitions, delete as soon as possible
7
#define DOUBLE   	0
7 8
#define SINGLE		1
8 9
#define ON_RAMP		2
9 10
#define OFF_RAMP	3
11
*/
10 12

  
13
#define SINGLE      0
14
#define ON_RAMP     1
15
#define OFF_RAMP    2
16
#define DOUBLE_C    3
17
#define DOUBLE_T    4
18

  
11 19
#define ISTRAIGHT	0
12 20
#define ILEFT		1
13 21
#define IRIGHT		2
trunk/code/projects/linefollowing/lineFollow.c
1 1
/**
2 2
 * @file lineFollow.c
3
 * @defgroup lineFollwing Line Following
3 4
 *
4 5
 * Takes care of following a line. Running this program is done by calling the
5 6
 * init() function and then the lineFollow(speed) command.  However, direct use
6 7
 * of this class is discouraged as its behavior is used by lineDrive.c, which
7 8
 * extends this class to provide behavior functionality.
8 9
 *
9
 * @author Dan Jacobs
10
 * @author Dan Jacobs and the Colony Project
10 11
 * @date 11-1-2010
11 12
 */
12 13

  
13 14
#include "lineFollow.h"
14 15

  
16
//! The number of bits expected in a barcode
15 17
#define CODESIZE 5 
16 18

  
17 19
int countHi = 0;
......
21 23
int barCodePosition=0;
22 24

  
23 25
int turnDistance=0;
26
//! Counts the number of full line readings before we determine an intersection
24 27
int intersectionFilter=0;
25 28
int disableBarCode=0;
26 29

  
30
//! Keeps track of where the encoder of one motor started, for use in turns.
31
int encoderStart = -1;
32
int encoderReset = 0;   // 0 if encoderStart has no value set
27 33

  
28
/** 
29
 * Initializes line following. Must be called before other line-following
30
 * behavior will work.
31
 */
34

  
32 35
void lineFollow_init()
33 36
{
34 37
	analog_init(0);
......
54 57
	//not on line
55 58
	if(position == NOLINE)
56 59
	{
57
		if(lost++>20)
60
		if(lost++ > 20)
58 61
		{
59 62
			orb2_set_color(GREEN);
60 63
			motors_off();
......
63 66
	}
64 67
	else if(position == FULL_LINE)
65 68
	{
66
		if(intersectionFilter++>4)
69
		if(intersectionFilter++ > 4)
67 70
		{
68 71
			orb2_set_color(RED);
69 72
			barCodePosition=0;
......
153 156
/**
154 157
 * Turns left at a cross of two lines.  Assumes that we are at lines in a cross
155 158
 * pattern, and turns until it sets straight on the new line.
156
 * @return 1 if the turn was executed successfully, 0 otherwise.
159
 * @return 0 if turn finishes otherwise return 1
157 160
 */
158 161
int turnLeft()
159 162
{
160
        motor_l_set(BACKWARD, 200);
163
        /*motor_l_set(BACKWARD, 200);
161 164
        motor_r_set(FORWARD, 200);
162 165
        int colors[5];
163 166
        updateLine(colors);
......
171 174
                         return 0;
172 175
                }
173 176
        }
177
        return 1;*/
178

  
179
        motor_l_set(BACKWARD,200);
180
        motor_r_set(FORWARD,200);
181
        if(!encoderReset)
182
        {
183
            encoderStart = encoder_get_x(RIGHT);
184
            encoderReset = 1;
185
        }
186

  
187
        if(encoder_get_x(RIGHT) < encoderStart)
188
        {
189
            encoderStart = 0;
190
            // Temporary: display an "error message" in case of overflow.
191
            // Using this for debugging, take it out soon!
192
            motor_l_set(FORWARD,0);
193
            motor_r_set(FORWARD,0);
194
            orb_set_color(WHITE);
195
            delay_ms(2000);
196
        }
197

  
198
        if(encoder_get_x(RIGHT) - encoderStart > 300)
199
        {
200
            encoderReset = 0;
201
            return 0;
202
        }
174 203
        return 1;
175 204
}
176 205

  
......
179 208
/**
180 209
 * Turns right at a cross of two lines.  Assumes that we are at lines in a cross
181 210
 * pattern, and turns until it sets straight on the new line.
182
 * @return 1 if the turn was executed successfully, 0 otherwise.
211
 * @return 0 if the turn finishes otherwise return 1
183 212
 */
184 213
int turnRight()
185 214
{
......
202 231

  
203 232

  
204 233

  
205

  
206 234
int getBarCode()
207 235
{
208 236
	if(barCodePosition!=CODESIZE) return NOBARCODE ;
209 237
	int temp = 0;
210
	int i;
211
	for(i=0; i<CODESIZE; i++)
238
	for(int i=0; i<CODESIZE; i++)
212 239
		temp += (barCode[i] << i);
213 240
	barCodePosition = 0;
214 241
	return temp;
......
240 267
		return NOLINE;	
241 268
	if(count==5)
242 269
		return FULL_LINE;
243
	return (wsum/count)-4;
270
	return (wsum/count)-4; // Subtract 4 to center the index around the center.
244 271
}
245 272

  
246 273

  
247 274
void updateBarCode()
248 275
{
276
	//! Note: currently only uses one of the barcode sensors.
249 277

  
250
	//NOTE: currently only uses one of the barcode sensors.
251

  
252 278
	//maps the sensors to the analog input ports
253 279
	int ports[2] = {8,1};
254 280
	int current[2];
......
284 310
}
285 311

  
286 312

  
287
//! A simple function to return the minimum of two integers.
288 313
int min(int x, int y){return x>y ? y : x;}
289
//! A simple function to return the maximum of two integers.
290 314
int max(int x, int y){return x<y ? y : x;}
291 315

  
292 316
void motorLeft(int speed){

Also available in: Unified diff