Project

General

Profile

Revision 1871

Added by John Sexton over 13 years ago

Branched the trunk into the linefollowing folder in branches.

View differences:

branches/linefollowing/projects/traffic_navigation/main.c
1
/*
2
 * main.c for Traffic Navigation
3
 * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
4
 *
5
 * Author: Colony Project, CMU Robotics Club
6
 */
7

  
8
#include <dragonfly_lib.h>
9
#include <wl_basic.h>
10
#include "../linefollowing/lineDrive.h"
11

  
12

  
13

  
14
/*States*/
15
#define SROAD 0
16
#define SINTERSECTION 10
17
#define SHIGHWAY 20
18

  
19
/*Sign Codes
20
 * bitwise OR labels to create a barcode or read one
21
 * There should be macros to extract these probably
22
 * The bits will be stored in some variable (char or short)
23
 * Bits if road: ? ? ? NAME NAME NAME TYPE CROAD
24
 * Bits if intersection: ? ? ? ? DIR DIR #WAYS CINTERSECTION
25
 */
26
#define CROAD 0x0 //0b
27
#define CINTERSECTION 0x1 //1b
28
#define CNORMALROAD 0x0 //00b
29
#define CHIGHWAYROAD 0x2 //10b
30
#define C4WAY 0x0 //00b
31
#define C3WAY 0x2 //10b
32
#define CNORTH 0x0 //0000b
33
#define CEAST 0x4 //0100b
34
#define CSOUTH 0x8 //1000b
35
#define CWEST 0x12 //1100b
36

  
37
/*Wireless Packet Types
38
 * The first byte of any wireless packet should be one of these types.
39
 * Each type will have its own structure
40
 * The second byte should be the id of the bot sending the packet
41
 * The third byte should be the number of the intersection or road that
42
 *   the packet pertains to
43
 */
44
#define PACKET_LENGTH 5
45
#define WROADENTRY 0 //[type, bot, road]
46
#define WROADREPLY 1 //[type, fromBot, road, toBot]
47
#define WROADEXIT 2 //[type, bot, road]
48
#define WROADSTOP 3 //[type, bot, road]
49
#define WINTERSECTIONENTRY 10 //[type, bot, intersection, fromDir, toDir]
50
#define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot]
51
#define WINTERSECTIONEXIT 12 //[type, bot, intersection]
52
#define WINTERSECTIONGO 13 //[type, bot, intersection]
53
#define WINTERSECTIONPOLICEENTRY 14
54
#define WHIGHWAYENTRY 20 //[type, bot, highway]
55
#define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot]
56
#define WHIGHWAYEXIT 22 //[type, bot, highway]
57
#define WPINGGLOBAL 30 //[type, bot]
58
#define WPINGBOT 31 //[type, fromBot, toBot]
59
#define WPINGQUEUE 32 //[type, fromBot, toBot]
60
#define WPINGREPLY 33 //[type, fromBot, toBot]
61

  
62
/*Macros
63
 */
64
#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE)
65

  
66
int main (void) {
67

  
68
	int state, sign, dataLength, pingWaitTime;
69
	char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum;
70
	unsigned char *packet;
71

  
72
	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
73
	dragonfly_init(ALL_ON);
74
	xbee_init();
75
	encoders_init();
76
//	lineDrive_init();
77
	rtc_init(SIXTEENTH_SECOND, NULL);	
78
	wl_basic_init_default();
79
	wl_set_channel(13);
80
	
81
	id = get_robotid();
82
	sign = 0;
83
	
84
	//Test code
85
	state = SROAD;
86

  
87
	sendBuffer[1] = id;
88

  
89
	while (1) {
90
		/*DTM Finite State Machine*/
91
		switch(state){
92
		case SROAD:/*Following a normal road*/
93
//			sign = lineFollow();
94
			//other road behaviors
95
				//tailgating?
96
			//read barcode
97
//			doDrive(255);
98
			if((sign & CINTERSECTION) || button1_click()){
99
				state = SINTERSECTION;
100
			}
101
			break;
102
		case SINTERSECTION:/*Entering, and in intersection*/
103
			//Intersection behaviors
104
				//queueing
105
				//no-stop?
106
			//check wireless
107
			
108
			intersectionNum = 0;
109

  
110
			/*Intersection queue:
111
			 *Each robot when entering the intersection will check for other robots
112
			 *in the intersection, and insert itself in a queue to go through.
113
			 */
114
			prevBot = 0;
115
			nextBot = 0;
116
			//Sends packet announcing its entry to the intersection
117
			sendBuffer[0] = WINTERSECTIONENTRY;
118
			sendBuffer[2] = intersectionNum;//Intersection #
119
			sendBuffer[3] = 0;//From Direction
120
			sendBuffer[4] = 2;//To Direction
121
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
122
			orb1_set_color(BLUE);
123
//			stop();
124
			rtc_reset();
125
			while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
126
				dataLength = 0;
127
				packet = wl_basic_do_default(&dataLength);
128
				if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
129
					if(packet[0] == WINTERSECTIONREPLY){
130
						if(packet[3] == id){//Reply for me
131
							prevBot = packet[1];
132
							orb2_set_color(GREEN);
133
							break;
134
						}else if(packet[1] != id){//Someone else got here first, try again
135
							sendBuffer[0] = WINTERSECTIONENTRY;
136
							sendBuffer[2] = intersectionNum;//Intersection #
137
							sendBuffer[3] = 0;//From Direction
138
							sendBuffer[4] = 2;//To Direction
139
							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
140
							orb2_set_color(ORANGE);
141
							rtc_reset();
142
						}
143
					}else /*if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
144
						sendBuffer[0] = WINTERSECTIONREPLY;
145
						sendBuffer[2] = intersectionNum;
146
						sendBuffer[3] = packet[1];
147
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
148
						nextBot = packet[1];
149
						nextDir = packet[3];
150
						nextPath = packet[4];
151
						orb2_set_color(BLUE);
152
						delay_ms(200);
153
					}*/
154
						delay_ms(0);
155
				}
156
			}
157
			orb1_set_color(PURPLE);
158
			//waits for its turn
159
			while(prevBot != 0){
160
				dataLength = 0;
161
				packet = wl_basic_do_default(&dataLength);
162
				if(dataLength==PACKET_LENGTH){
163
					if(packet[2] == intersectionNum){
164
						if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
165
							prevBot = 0;
166
							orb2_set_color(PURPLE);
167
						}/*else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
168
							sendBuffer[0] = WINTERSECTIONREPLY;
169
							sendBuffer[2] = intersectionNum;
170
							sendBuffer[3] = packet[1];
171
							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
172
							nextBot = packet[1];
173
							nextDir = packet[3];
174
							nextPath = packet[4];
175
							orb2_set_color(BLUE);
176
						}*/
177
					}
178
/*					if(ISPING(packet)){
179
						sendBuffer[0] = WPINGREPLY;
180
						sendBuffer[2] = packet[1];
181
						if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
182
							nextBot = packet[1];
183
						}
184
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
185
					}
186
					if(packet[0]==WPINGREPLY && packet[2]==id){
187
						prevBot = packet[1];
188
						pingWaitTime = rtc_get();
189
					}
190
*/				}
191
							
192
				if(prevBot && rtc_get() << 12 == 0){
193
					sendBuffer[0] = WPINGBOT;
194
					sendBuffer[2] = prevBot;
195
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
196
					pingWaitTime = rtc_get();
197
				}
198
				if(prevBot && pingWaitTime - rtc_get() > 4){
199
					sendBuffer[0] = WPINGBOT;
200
					if(pingWaitTime - rtc_get() > 8){
201
						sendBuffer[0] = WPINGQUEUE;
202
					}
203
					sendBuffer[2] = prevBot;
204
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
205
				}
206
/*				if(prevBot && pingWaitTime - rtc_get() > 12){
207
					prevBot = 0;
208
				}
209
*/			}
210
			orb1_set_color(RED);
211
			//Drives through intersection
212
			//Code to choose path through intersection goes in this while loop
213
			//But here's the code to handle wireless while in the intersection...
214
//			start();
215
//			turn(DOUBLE, ILEFT);//Change paramers to variables
216
			while(!button1_click()/*replace with variable to keep track of if in intersection*/){
217
//				doDrive(255);
218
				packet = wl_basic_do_default(&dataLength);		
219
				if(dataLength==PACKET_LENGTH){
220
					if(packet[2]==intersectionNum/*Intersection Num*/){
221
						if(packet[0]==WINTERSECTIONEXIT){
222
							prevBot = 0;
223
							orb2_set_color(RED);
224
						}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
225
							sendBuffer[0] = WINTERSECTIONREPLY;
226
							sendBuffer[2] = intersectionNum;//Intersection #
227
							sendBuffer[3] = packet[1];
228
							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
229
							nextBot = packet[1];
230
							nextDir = packet[3];
231
							nextPath = packet[4];
232
							orb2_set_color(YELLOW);
233
						}
234
					}
235
					if(ISPING(packet)){
236
						sendBuffer[0] = WPINGREPLY;
237
						sendBuffer[2] = packet[1];
238
						if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
239
							nextBot = packet[1];
240
						}
241
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
242
					}
243
				}
244
				if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
245
					/*if(bots paths won't collide){
246
						sendBuffer[0] = WINTERSECTIONGO;
247
						sendBuffer[2] = 0;//Intersection #
248
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
249
					*/
250
				}
251
			}
252
			//Exits intersection
253
			sendBuffer[0] = WINTERSECTIONEXIT;
254
			sendBuffer[2] = intersectionNum;//Intersection #
255
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
256
			orb1_set_color(ORANGE);
257
			
258
			if(!(sign & CINTERSECTION)){
259
				if(sign & CHIGHWAYROAD){
260
					state = SHIGHWAY;
261
				}else{
262
					state = SROAD;
263
				}
264
			}
265
			break;
266
		case SHIGHWAY:/*On highway*/
267
//			sign = lineFollow();
268
			//highway behaviors
269
				//merging
270
				//passing
271
			//read barcode
272
			break;
273
		default:
274
			usb_puts("I got stuck in an unknown state! My state is ");
275
			usb_puti(state);
276
		}
277
	}
278

  
279
}
branches/linefollowing/projects/traffic_navigation/validTurns.h
1
#ifndef _VALID_TURNS_
2
#define _VALID_TURNS_
3

  
4
#include "lineFollow.h"
5
#include "lineDrive.h"
6

  
7
//To differenciate between T and Cross Intersections
8
#define DOUBLE_C	4
9
#define DOUBLE_T	5
10

  
11
//DOUBLE intersection (Cross Intersection) positions
12
//use T-Junction positions
13

  
14
//DOUBLE intersection (T-Junction) positions
15
/*
16
TLEFT=======TRIGHT
17
	||
18
	||
19
	||
20
	TMIDDLE
21
*/
22
#define TLEFT		0			
23
#define TRIGHT		1			
24
#define TMIDDLE		2
25

  
26
//SINGLE intersection positions
27
/*
28
	   /\
29
	  /||\
30
	   ||
31
SACROSS==========>
32
	   ||
33
	   ||
34
	   ||
35
	   SUP
36
*/
37
#define SACROSS		0
38
#define SUP		1
39

  
40

  
41
//ON_RAMP and OFF_RAMP intersection positions
42
/*
43
R_LEFT================R_RIGHT
44
	    ||	
45
	    ||
46
	    ||
47
	  R_RAMP
48
*/
49
#define R_LEFT		0
50
#define R_RIGHT		1
51
#define R_RAMP		2
52

  
53
int getCrossType(int barcode);
54

  
55
int getCrossPos(int barcode, int max);
56

  
57
int getTurnType();
58

  
59
int validateTurn(int barcode);
60

  
61
#endif
branches/linefollowing/projects/traffic_navigation/validTurns.c
1
/*
2
 * Deterministic turning implemented using randomness. Using a
3
 * random number generator, the robot decides to go straight, 
4
 * left, right, or u-turn.
5
 *
6
 */
7

  
8

  
9
#include "validTurns.h"
10

  
11
int randomNumGen(int max){
12
	int x = range_read_distance(IR2);
13
	if (x>0) return range_read_distance(IR2)%max;
14
	else return randomNumGen(max);
15
}
16

  
17
int getCrossType(int barcode)
18
{
19
	int x = randomNumGen(4);
20
	if (x == DOUBLE) {
21
		int y = randomNumGen(2);
22
		if (y == 0) x = DOUBLE_C;
23
		else x = DOUBLE_T;
24
	}
25
	return x;
26
}
27

  
28
int getCrossPos(int barcode, int max)
29
{
30
	return randomNumGen(max);
31
}
32

  
33
int getTurnType()
34
{
35
	return randomNumGen(4);
36
}
37

  
38
int validateTurn(int barcode)
39
{
40
	int cross_type;
41
	int cross_pos;
42
	if( barcode == NOBARCODE ) return -1;
43
	cross_type = getCrossType(barcode);
44
	switch (cross_type)
45
	{
46
		case DOUBLE_C:
47
		{
48
			cross_pos = getCrossPos(barcode, 4);
49
			if (0<=cross_pos && cross_pos<=3)
50
			return randomNumGen(4);
51
		break;
52
		}
53
		case DOUBLE_T:
54
		{
55
			cross_pos = getCrossPos(barcode, 3);
56
			switch (cross_pos)
57
			{
58
				case TLEFT:
59
				{
60
					int turn_type = getTurnType();
61
					if (turn_type == ILEFT) turn_type = ISTRAIGHT;
62
					return turn_type;	
63
				break;
64
				}
65
				case TRIGHT:
66
				{
67
					int turn_type = getTurnType();
68
					if (turn_type == IRIGHT) turn_type = ISTRAIGHT;
69
					return turn_type;			
70
				break;
71
				}
72
				case TMIDDLE:
73
				{
74
					int turn_type = getTurnType();
75
					if (turn_type == ISTRAIGHT) turn_type = IUTURN;
76
					return turn_type;
77
				break;
78
				}
79
				default:
80
					return -1;
81
			}
82
			break;
83
		}
84
		case SINGLE:
85
		{
86
			cross_pos = getCrossPos(barcode, 2);
87
			switch (cross_pos)
88
			{
89
				case SACROSS:
90
				{
91
					int turn_type = getTurnType();
92
					if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
93
					return turn_type;	
94
				break;
95
				}
96
				case SUP:
97
				{
98
					int turn_type = getTurnType();
99
					if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
100
					return turn_type;			
101
				break;
102
				}
103
				default:
104
					return -1;
105
			}
106
		break;
107
		}
108
		case ON_RAMP:
109
		{
110
			cross_pos = getCrossPos(barcode, 3);
111
			switch (cross_pos)
112
			{
113
				case R_LEFT:
114
				{
115
					int turn_type = getTurnType();
116
					if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
117
					return turn_type;	
118
				break;
119
				}
120
				case R_RIGHT:
121
				{
122
					int turn_type = getTurnType();
123
					if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
124
					return turn_type;			
125
				break;
126
				}
127
				default:
128
					return -1;
129
			}
130
		break;
131
		}
132
		case OFF_RAMP:
133
		{
134
			cross_pos = getCrossPos(barcode, 3);
135
			switch (cross_pos)
136
			{
137
				case R_LEFT:
138
				{
139
					int turn_type = ISTRAIGHT;
140
					return turn_type;	
141
				break;
142
				}
143
				case R_RIGHT:
144
				{
145
					int turn_type = ISTRAIGHT;
146
					return turn_type;			
147
				break;
148
				}
149
				case R_RAMP:
150
				{
151
					int turn_type = getTurnType();
152
					if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT;
153
					return turn_type;
154
				break;
155
				}
156
				default:
157
					return -1;
158
			}
159
		break;
160
		}
161
	default:
162
		return -1;
163
}
164
}
branches/linefollowing/projects/traffic_navigation/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/linefollowing/projects/linefollowing/lineDrive.c
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){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;}
158

  
159
	//Should never get here
160
	return ERROR;
161
}
162

  
branches/linefollowing/projects/linefollowing/lineFollow.c
1
/**
2
 * @file lineFollow.c
3
 *
4
 * Takes care of following a line. Running this program is done by calling the
5
 * init() function and then the lineFollow(speed) command.  However, direct use
6
 * of this class is discouraged as its behavior is used by lineDrive.c, which
7
 * extends this class to provide behavior functionality.
8
 *
9
 * @author Dan Jacobs
10
 * @date 11-1-2010
11
 */
12

  
13
#include "lineFollow.h"
14

  
15
#define CODESIZE 5 
16

  
17
int countHi = 0;
18
int countLo = 0;
19
int maxAvg, avg;
20
int barCode[ CODESIZE ];
21
int barCodePosition=0;
22

  
23
int turnDistance=0;
24
int intersectionFilter=0;
25
int disableBarCode=0;
26

  
27

  
28
/** 
29
 * Initializes line following. Must be called before other line-following
30
 * behavior will work.
31
 */
32
void lineFollow_init()
33
{
34
	analog_init(0);
35
	lost = 0;
36
	intersectionFilter=0;
37
	disableBarCode=0;
38
}
39

  
40

  
41
/** 
42
 * Follows a line at the given speed.
43
 * @param speed The speed with which to follow the line.
44
 */
45
int lineFollow(int speed)
46
{
47
	int colors[5];
48
	int position;
49
	
50

  
51
	updateLine(colors);
52
	position = lineLocate(colors); 	
53
	
54
	//not on line
55
	if(position == NOLINE)
56
	{
57
		if(lost++>20)
58
		{
59
			orb2_set_color(GREEN);
60
			motors_off();
61
			return LINELOST;
62
		}
63
	}
64
	else if(position == FULL_LINE)
65
	{
66
		if(intersectionFilter++>4)
67
		{
68
			orb2_set_color(RED);
69
			barCodePosition=0;
70
			disableBarCode=50;
71
		}
72
	}
73
	//on line
74
	else
75
	{
76
		position*=30;
77
		orb2_set_color(ORB_OFF);
78
		motorLeft(min(speed+position, 255));
79
		motorRight(min(speed-position, 255));
80
		lost=0;
81
		intersectionFilter=0;
82
	}
83

  
84
	if(disableBarCode--)
85
	{
86
		if(disableBarCode)return NOBARCODE;
87
		return INTERSECTION;
88
	}
89
	updateBarCode();
90
	return getBarCode();
91
}
92

  
93

  
94
/**
95
 * Implements the left merge, assuming a line exists to the left.  Works by
96
 * turning off the line at an increasing angle and waiting to hit another line
97
 * on the left.
98
 */
99
int mergeLeft()
100
{
101
	motor_l_set(FORWARD, 200);
102
	if(turnDistance!=21)motor_r_set(FORWARD, 230);
103
	else motor_r_set(FORWARD, 210);
104
	int colors[5];
105
	updateLine(colors);
106
	int position = lineLocate(colors);
107
	if(position>3 || position<-3)turnDistance++;
108

  
109
	if(turnDistance>20)
110
	{
111
	turnDistance=21;
112
	
113
		if(position<3 && position>-3)
114
		{
115
			turnDistance = 0;
116
			return 0;
117
		}	
118
	}
119
	return 1;
120
}
121

  
122

  
123
/**
124
 * Implements the right merge, assuming a line exists to the right.  Works by
125
 * turning off the line at an increasing angle and waiting to hit another line
126
 * on the right.
127
 */
128
int mergeRight()
129
{
130
        motor_r_set(FORWARD, 200);
131
        if(turnDistance!=21)motor_l_set(FORWARD, 230);
132
        else motor_l_set(FORWARD, 210);
133
        int colors[5];
134
        updateLine(colors);
135
        int position = lineLocate(colors);
136
        if(position>3 || position<-3)turnDistance++;
137

  
138
        if(turnDistance>20)
139
        {
140
        turnDistance=21;
141

  
142
                if(position<3 && position>-3)
143
                {
144
                        turnDistance = 0;
145
                        return 0;
146
                } 
147
        }
148
        return 1;
149
}
150

  
151

  
152

  
153
/**
154
 * Turns left at a cross of two lines.  Assumes that we are at lines in a cross
155
 * pattern, and turns until it sets straight on the new line.
156
 * @return 1 if the turn was executed successfully, 0 otherwise.
157
 */
158
int turnLeft()
159
{
160
        motor_l_set(BACKWARD, 200);
161
        motor_r_set(FORWARD, 200);
162
        int colors[5];
163
        updateLine(colors);
164
        int position = lineLocate(colors);
165
        if(position>2 || position<-2)turnDistance++;
166
        if(turnDistance>1)
167
        {
168
                if(position<3 && position>-3)
169
                {
170
                        turnDistance = 0;
171
                         return 0;
172
                }
173
        }
174
        return 1;
175
}
176

  
177

  
178

  
179
/**
180
 * Turns right at a cross of two lines.  Assumes that we are at lines in a cross
181
 * pattern, and turns until it sets straight on the new line.
182
 * @return 1 if the turn was executed successfully, 0 otherwise.
183
 */
184
int turnRight()
185
{
186
        motor_r_set(BACKWARD, 200);
187
        motor_l_set(FORWARD, 200);
188
        int colors[5];
189
        updateLine(colors);
190
        int position = lineLocate(colors);
191
        if(position>2 || position<-2)turnDistance++;
192
        if(turnDistance>1) 
193
	{
194
		if(position<3 && position>-3)
195
		{
196
			turnDistance = 0;
197
			 return 0;
198
		}
199
	}
200
	return 1;
201
}
202

  
203

  
204

  
205

  
206
int getBarCode()
207
{
208
	if(barCodePosition!=CODESIZE) return NOBARCODE ;
209
	int temp = 0;
210
	int i;
211
	for(i=0; i<CODESIZE; i++)
212
		temp += (barCode[i] << i);
213
	barCodePosition = 0;
214
	return temp;
215
}
216

  
217

  
218

  
219
void updateLine(int* values)
220
{	
221
	int ports[5] = {13, 12, 3, 2, 9};
222
	for(int i = 0; i<5; i++)
223
		values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
224
}
225

  
226

  
227

  
228
int lineLocate(int* colors)
229
{
230
	int i;
231
	int wsum = 0;
232
	int count=0;
233

  
234
	for(i = 0; i<5; i++)
235
	{
236
		count += colors[i]/2;
237
		wsum += (i)*colors[i];
238
	}
239
	if(count==0)
240
		return NOLINE;	
241
	if(count==5)
242
		return FULL_LINE;
243
	return (wsum/count)-4;
244
}
245

  
246

  
247
void updateBarCode()
248
{
249

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

  
252
	//maps the sensors to the analog input ports
253
	int ports[2] = {8,1};
254
	int current[2];
255
//	current[0] = analog_get10(ports[0]);
256
	current[1] = analog_get10(ports[1]);
257

  
258
	if(current[1]>500)
259
	{
260
		if(countHi++==0) 
261
		{
262
			avg = 500;
263
			maxAvg = 500;
264
		}
265
		else
266
		{
267
			avg = 3*avg + current[1];
268
			avg/=4;
269
			maxAvg = max(maxAvg, avg);
270
		}
271
	}
272
	else if(countHi>5)
273
	{
274
		if(countLo++>15)
275
		{
276
			countHi=countLo=0;
277
			if(maxAvg>825)orb1_set_color(RED);
278
			else orb1_set_color(BLUE);
279
			barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
280
		}
281
	}
282
	else countHi/=2;
283
	if(countHi==0)countLo=0; 
284
}
285

  
286

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

  
292
void motorLeft(int speed){
293
	((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127);
294
}
295

  
296
void motorRight(int speed){
297
        ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127);
298
}
branches/linefollowing/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
branches/linefollowing/projects/linefollowing/lineFollow.h
1
#include <dragonfly_lib.h>
2

  
3
 #ifndef _LINEFOLLOW_H_
4
 #define _LINEFOLLOW_H_
5

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

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

  
17

  
18

  
19
/* 	lineFollow_init
20
	Must call before lineFollow
21
	Turns analog loop off
22
*/ 
23
void lineFollow_init(void);
24

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

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

  
40
/*	updateLine
41
	Reads in the analog values
42
	Fills the given array with WHITE
43
	or BLACK representing the line
44
*/
45
void updateLine(int* values); 
46

  
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
*/
52
int lineLocate(int* colors);
53

  
54
/*	updatebarCode
55
	Reads in and processes
56
	bar code data
57
*/
58
void updateBarCode(void);
59

  
60
/*	getBarCode
61
	returns a bar code, if
62
	available, otherwise NOBARCODE
63
*/
64
int getBarCode(void);
65

  
66
/*	min max
67
	returns the minimum/maximum of two values
68
*/
69
int min(int x, int y);
70
int max(int x, int y);
71

  
72
/*	motorLeft
73
	Commands the left motor
74
	Cannot be used to stop
75
	0-126 are backward
76
	127-255 are forward
77
*/
78
void motorLeft(int speed);
79

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

  
88
/*	lost
89
	Internal counter to detect if the line was lost
90
*/
91
int lost;
92

  
93
#endif
branches/linefollowing/projects/linefollowing/main.c
1
#include <dragonfly_lib.h>
2
#include <lineFollow.h>
3

  
4

  
5

  
6
int main(void)
7
{
8

  
9
	/* initialize components, set wireless channel */
10
	dragonfly_init(ALL_ON);
11
	lineFollow_init();
12
	int barCode;
13
	while(1)
14
	{
15
		
16

  
17
		for(int q=0; q<500; q++)
18
			barCode = lineFollow(200);
19
		while(mergeLeft());
20
		for(int q=0; q<1000; q++)lineFollow(200);
21
		
22
		continue;
23
		if(barCode==-25)while(turnRight());
24
		continue;
25
		if(barCode != -2 && barCode != LINELOST)
26
		{
27
			usb_puti(barCode);
28
			usb_putc('\n');
29
		}
30
/*		
31

  
32
		switch (barCode)
33
		{	
34

  
35
		
36
			case 0: orb_set_color(RED); break;
37
			case 1: orb_set_color(ORANGE); break;
38
			case 2: orb_set_color(YELLOW); break;
39
			case 3: orb_set_color(LIME); break;
40
			case 4: orb_set_color(GREEN); break;
41
			case 5: orb_set_color(CYAN); break;
42
			case 6: orb_set_color(BLUE); break;
43
			case 7: orb_set_color(PINK); break;
44
			case 8: orb_set_color(PURPLE); break;
45
			case 9: orb_set_color(MAGENTA); break;
46
			default: orb_set_color(WHITE); break;
47
	
48
			case 0:
49
				straight();
50
				turnRight();
51
				break;
52
                        case 2:
53
	                        straight();
54
        	                break;
55
                        case 3:
56
	                        straight();
57
				turnLeft();
58
        	                break;
59
		
60
		}
61
*/	
62
	}
63
	return 0;
64
}
65
void right()
66
{
67
motor_r_set(BACKWARD, 200);
68
motor_l_set(FORWARD, 200);
69
delay_ms(400);
70
}
71

  
72

  
73
void straight()
74
{
75
motor_r_set(FORWARD, 210);
76
motor_l_set(FORWARD, 210);
77
delay_ms(200);
78
move(0,0);
79
delay_ms(2000);
80
}
81

  
82
void left()
83
{
84
motor_l_set(BACKWARD, 200);
85
motor_r_set(FORWARD, 200);
86
delay_ms(400);
87
}
branches/linefollowing/projects/linefollowing/Makefile
1
# this is a local makefile
2

  
3
# Relative path to the root directory (containing lib directory)
4
ifndef COLONYROOT
5
COLONYROOT := ..
6

  
7
# Target file name (without extension).
8
TARGET = main
9

  
10
# Uncomment this to use the wireless library
11
USE_WIRELESS = 1
12

  
13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15

  
16
else
17
COLONYROOT := ../$(COLONYROOT)
18
endif
19

  
20
include $(COLONYROOT)/Makefile
branches/linefollowing/projects/libdragonfly/analog.c
1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26
/**
27
 * @file analog.c
28
 * @brief Analog input and output
29
 *
30
 * Contains functions for manipulating the ADC on the Dragonfly board.
31
 * 
32
 * @author Colony Project, CMU Robotics Club
33
 * originally taken from fwr analog file (author: Tom Lauwers)
34
 * loop code written by Kevin Woo and James Kong
35
 **/
36

  
37
#include <util/delay.h>
38
#include <avr/interrupt.h>
39
#include "analog.h"
40
#include "serial.h"
41
// Internal Function Prototypes
42
void set_adc_mux(int which);
43

  
44
/**
45
 * @defgroup analog Analog
46
 * Functions for manipulation the ADC on the dragonfly board.
47
 * All definitions may be found in analog.h.
48
 *
49
 * @{
50
 **/
51

  
52
volatile int adc_loop_status = ADC_LOOP_STOPPED;
53
volatile int adc_sig_stop_loop = 0;
54
volatile int adc_current_port = 0;
55
volatile adc_t an_val[11];
56

  
57
/**
58
 * Initializes the ADC.
59
 * Call analog_init before reading from the analog ports.
60
 *
61
 * @see analog8, analog10, analog_get8, analog_get10
62
 *
63
 * @bug First conversion takes a performance penalty of
64
 * 25 vs. 13 ADC clock cycles of successive conversions.
65
 * Analog_init should run a dummy conversion to pre-empt
66
 * this.
67
 *
68
 * For good 10-bit precision, ACD clock must be between
69
 * 50kHz and 200kHz. Currently, ADC clock is fixed at
70
 * 125kHz using 1/64prescalar. However, most code uses
71
 * 8-bit precision which can work at ADC clock speeds
72
 * higher than 200kHz. Experimental tests needed to
73
 * determine highest clock speed for accurate 8-bit ADC.
74
 *
75
 **/
76
void analog_init(int start_conversion) {
77
	start_conversion=0;                   //forces the analog loop off
78
	for (int i = 0; i < 11; i++) {
79
		an_val[i].adc10 = 0;
80
		an_val[i].adc8 = 0;
81
	}
82

  
83
	// ADMUX register
84
	// Bit 7,6 - Set voltage reference to AVcc (0b01)
85
	// Bit 5 - ADLAR set to simplify moving from register
86
	// Bit 4 - X
87
	// Bit 3:0 - Sets the current channel
88
	// Initializes to read from AN1 first (AN0 is reservered for the BOM)
89
	ADMUX = 0;
90
	ADMUX |= ADMUX_OPT | _BV(MUX0);
91

  
92
	// ADC Status Register A
93
	// Bit 7 - ADEN is set (enables analog)
94
	// Bit 6 - Start conversion bit is set (must be done once for free-running mode)
95
	// Bit 5 - Enable Auto Trigger (for free running mode) NOT DOING THIS RIGHT NOW
96
	// Bit 4 - ADC interrupt flag, 0
97
	// Bit 3 - Enable ADC Interrupt (required to run free-running mode)
98
	// Bits 2-0 - Set to create a clock divisor of 2, to make ADC clock != 8,000,000/64 = 125kHz (it runs at highest frequency)
99
	ADCSRA = 0;
100
	ADCSRA |= _BV(ADEN) | /*_BV(ADPS2) | _BV(ADPS1) |*/ _BV(ADPS0);
101
	
102
	// Set external mux lines to outputs
103
	DDRG |= 0x1C;
104
	
105
	// Set up first port for conversions
106
	set_adc_mux(0x00);
107
	adc_current_port = AN1;
108

  
109
	//Start the conversion loop if requested
110
	if (start_conversion)
111
		analog_start_loop();
112
		
113
	//Conversion loop disabled by default
114
}	
115

  
116
/**
117
 * Returns the 8-bit analog conversion of which from
118
 * the lookup table. If the requested port is the BOM_PORT
119
 * you will get an automatic 0 since the BOM_PORT is not
120
 * read in the loop and not stored. If you need that port
121
 * you should use the functions in bom.c. There is an analog_get8
122
 * function which for instant lookups but should be avoided unless
123
 * you know what you're doing.
124
 *
125
 * @param which the port that you want to read
126
 *
127
 * @bug may cause a seg fault if which is a larger value
128
 * than exists in an_val table. Not sure if we should fix
129
 * this or not since it would add overhead.
130
 *
131
 * @return 8-bit analog value for the which port requested
132
 *
133
 * @see analog10, analog_get8, analog_get10
134
 **/
135
unsigned int analog8(int which) {
136
/*	if (which == BOM_PORT) {
137
		return 0;
138
	} else {
139
		return an_val[which - 1].adc8;
140
	}
141
*/
142
return analog_get8(which);
143
}
144

  
145
/**
146
 * Returns the 10-bit analog conversion of which from
147
 * the lookup table. If the requested port is the BOM_PORT
148
 * you will get an automatic 0 since the BOM_PORT is not
149
 * read in the loop and not stored. If you need that port
150
 * you should use the functions in bom.c. There is an analog_get10
151
 * function which for instant lookups but should be avoided unless
152
 * you know what you are doing.
153
 *
154
 * @param which the port that you want to read
155
 *
156
 * @bug may cause a seg fault if which is a larger value
157
 * than exists in an_val table. Not sure if we should fix
158
 * this or not since it would add overhead.
159
 *
160
 * @return 10-bit analog value for the which port requested
161
 *
162
 * @see analog8, analog_get8, analog_get10
163
 **/
164
unsigned int analog10(int which) {
165
/*	if (which == BOM_PORT) {
166
		return 0;
167
	} else {
168
		return an_val[which - 1].adc10;
169
	}
170
*/
171
return analog_get10(which);
172
}
173

  
174
/**
175
 * Starts the analog update loop. Will continue to run
176
 * until analog_stop_loop is called.
177
 *
178
 * @see analog_stop_loop, analog_loop_status
179
 **/
180
void analog_start_loop(void) {
181
	if(adc_loop_status != ADC_LOOP_RUNNING){
182
		//Start the conversion, enable ADC interrupt
183
		ADCSRA |= _BV(ADIE);
184
		ADCSRA |= _BV(ADSC);
185
		adc_loop_status = ADC_LOOP_RUNNING;
186
	}
187
}
188

  
189
/**
190
 * Stops the analog update loop. If there is a current
191
 * read, it will finish up and be stored before the loop
192
 * is interrupted. No further updates will be made until
193
 * the loop is started again.
194
 *
195
 * @see analog_start_loop, analog_loop_status
196
 **/
197
void analog_stop_loop() {
198
	//Signal to stop after the next conversion
199
	adc_sig_stop_loop = 1;
200
}
201

  
202
/**
203
 * Returns the status of loop. 0 for stopped.
204
 * 1 for running. 2 for paused.
205
 *
206
 * @see analog_start_loop, analog_stop_loop
207
 **/
208
int analog_loop_status(void) {
209
	return adc_loop_status;
210
}
211

  
212
/**
213
 * Reads an 8-bit number from an analog port.
214
 * analog_init must be called before using this function.
215
 * The analog loop must also be stopped before using this
216
 * function or you will mess up the lookup table. You
217
 * must also reenabled the loop when you are done unless
218
 * you are doing more instant reads. See analog_stop_loop
219
 * and analog_start_loop for more information about the loop.
220
 * 
221
 * @param which the analog port to read from. One of
222
 * the constants AN0 - AN7.
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff