Revision 1988
This is a working version of the code for the demo. It works on Bot 7, and partially on Bot 14. Due to the number of changes here, I'll list them by file:
traffic_navigation.h
I added a #define for sendGraph, so its main won't be compiled unless defined. Uncomment the #define line at the top of this file to turn its main on. Make sure you also comment out the #define for main-new. Also, has the new #define for the node wireless packet type.
readSerial.sh
I changed the output to print to stdout, instead of a file.
sendGraph.c
sendGraph.h
I modified the struct that stores and sends node data so it has a wireless packet type (80), and is 1 byte larger to hold it.
validTurns.c
One of the intersections on the map was placed incorrectly, and the robot drives off the line if it gets there. I added 2 if statements to control how the robot turns if it is at that intersection. The intersection has barcode numbers 13 and 16.
Makefile
I changed the target to main-new because that was what I was testing. If you're testing something else, make sure you change this line.
main-new.c
I changed the speed at which the robot drives at. I lowered it from 200 to 180, although this value depends on which bot it was used on. I also changed all of Alex's #ifdef debugging code into #define that takes up less space and makes the code look cleaner.
trunk/code/projects/traffic_navigation/traffic_navigation.h | ||
---|---|---|
3 | 3 |
/* Debug Options - These must go before includes */ |
4 | 4 |
#define MAIN_NEW |
5 | 5 |
#define ORB_INTERSECTION |
6 |
//#define SENDGRAPH |
|
6 | 7 |
|
7 | 8 |
#include <dragonfly_lib.h> |
8 | 9 |
#include <wl_basic.h> |
... | ... | |
65 | 66 |
#define WPINGQUEUE 32 //[type, fromBot, toBot] |
66 | 67 |
#define WPINGREPLY 33 //[type, fromBot, toBot] |
67 | 68 |
#define WCOLLISIONAVOID 41 //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces |
69 |
#define WGRAPHDATA 80 |
|
68 | 70 |
|
69 | 71 |
/*Wireless Parsing Status |
70 | 72 |
* For wireless parsing - status codes describing various errors/statuses with wireless |
trunk/code/projects/traffic_navigation/readSerial.sh | ||
---|---|---|
1 | 1 |
#/bin/bash |
2 |
touch graphData |
|
2 | 3 |
while true |
3 | 4 |
do |
4 | 5 |
read LINE < /dev/ttyUSB0 |
5 |
echo $LINE >> graphData |
|
6 |
# echo $LINE >> graphData |
|
7 |
echo $LINE |
|
6 | 8 |
done |
trunk/code/projects/traffic_navigation/sendGraph.c | ||
---|---|---|
2 | 2 |
#include <wl_basic.h> |
3 | 3 |
#include "sendGraph.h" |
4 | 4 |
|
5 |
#ifdef SENDGRAPH |
|
6 |
|
|
5 | 7 |
int main(void){ |
6 | 8 |
|
7 | 9 |
dragonfly_init(ALL_ON); |
... | ... | |
40 | 42 |
intersections[0] = a; |
41 | 43 |
intersections[1] = b; |
42 | 44 |
|
45 |
while(1){ |
|
43 | 46 |
sendIntersectionGraph(); |
47 |
orb1_set_color(BLUE); |
|
48 |
delay_ms(500); |
|
49 |
sendIntersectionGraph(); |
|
50 |
orb1_set_color(YELLOW); |
|
51 |
delay_ms(500); |
|
52 |
} |
|
53 |
|
|
54 |
return 0; |
|
44 | 55 |
} |
45 | 56 |
|
46 | 57 |
|
... | ... | |
57 | 68 |
void sendIntersectionGraph() { |
58 | 69 |
int i; |
59 | 70 |
node_union graph; |
71 |
graph.n.packetType = WGRAPHDATA; |
|
60 | 72 |
|
61 | 73 |
for(i=0; i< NUM_FEATURES; i++){ |
62 | 74 |
graph.n = intersections[i]; |
63 |
wl_basic_send_global_packet(42, graph.array, 12);
|
|
64 |
wl_basic_send_global_packet(42, "\n", 1);
|
|
75 |
wl_basic_send_global_packet(42, graph.array, 13);
|
|
76 |
// wl_basic_send_global_packet(42, "\r\n", 3);
|
|
65 | 77 |
} |
66 | 78 |
} |
79 |
|
|
80 |
#endif |
trunk/code/projects/traffic_navigation/sendGraph.h | ||
---|---|---|
1 | 1 |
#include "intersectData.h" |
2 |
#include "traffic_navigation.h" |
|
2 | 3 |
|
3 | 4 |
/* Data is sent in the following order |
4 | 5 |
* INT0_NUM OUTEDGE_0 OUTEDGE_1 ... \n |
... | ... | |
17 | 18 |
|
18 | 19 |
/* Representation of an intersection on the graph */ |
19 | 20 |
typedef struct { |
20 |
char type; /* Note that there are at most 5 intersection types */ |
|
21 |
char packetType; |
|
22 |
char type; /* Note that there are at most 5 intersection types */ |
|
21 | 23 |
char intNum; /* What is the intersection number */ |
22 | 24 |
char numOut; /* Note that we can have no more than 4 outgoing edges */ |
23 | 25 |
char outSeen; /* The number of the outgoing edges that we have seen for this intersection */ |
... | ... | |
27 | 29 |
/* A union that is used in wireless transmissions */ |
28 | 30 |
typedef union { |
29 | 31 |
node n; |
30 |
char array[12];
|
|
32 |
char array[13];
|
|
31 | 33 |
}node_union; |
32 | 34 |
|
33 | 35 |
/* This array holds all of the intersections that are represented in the graph |
trunk/code/projects/traffic_navigation/validTurns.c | ||
---|---|---|
64 | 64 |
int intersect_type; |
65 | 65 |
int intersect_pos; |
66 | 66 |
intersect_type = getIntersectType(barcode); |
67 |
switch (intersect_type) |
|
67 |
//TEMPORARY CODE FOR TEST MAP |
|
68 |
//INTERSECTION 2 (barcodes 13 and 16) was made incorrectly |
|
69 |
//If the bot reads barcode 16, it must turn left |
|
70 |
//If the bot reads barcode 13, it must go straight |
|
71 |
if(barcode == 16){ |
|
72 |
return ILEFT; |
|
73 |
} |
|
74 |
if(barcode == 13){ |
|
75 |
return ISTRAIGHT; |
|
76 |
} |
|
77 |
//END TEMP CODE |
|
78 |
|
|
79 |
switch (intersect_type) |
|
68 | 80 |
{ |
69 | 81 |
case DOUBLE_C: |
70 | 82 |
{ |
trunk/code/projects/traffic_navigation/Makefile | ||
---|---|---|
5 | 5 |
COLONYROOT := .. |
6 | 6 |
|
7 | 7 |
# Target file name (without extension). |
8 |
TARGET = sendGraph
|
|
8 |
TARGET = main-new
|
|
9 | 9 |
|
10 | 10 |
# Uncomment this to use the wireless library |
11 | 11 |
USE_WIRELESS = 1 |
12 | 12 |
|
13 | 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/tty.usbserial*'; fi)
|
|
14 |
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
|
|
15 | 15 |
|
16 | 16 |
else |
17 | 17 |
COLONYROOT := ../$(COLONYROOT) |
trunk/code/projects/traffic_navigation/main-new.c | ||
---|---|---|
1 | 1 |
/* |
2 | 2 |
* main.c for Traffic Navigation |
3 |
* Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
|
|
3 |
* Runs the highest level behavior for the Dynamic Traffic Navigation (DTN) SURG
|
|
4 | 4 |
* |
5 | 5 |
* Author: Colony Project, CMU Robotics Club |
6 | 6 |
*/ |
7 | 7 |
|
8 |
#define MAIN_NEW |
|
9 |
|
|
8 | 10 |
#include "traffic_navigation.h" |
9 | 11 |
#include "../linefollowing/lineDrive.h" |
10 |
#ifndef MAIN_NEW
|
|
12 |
#ifdef MAIN_NEW |
|
11 | 13 |
|
14 |
#ifdef ORB_INTERSECTION |
|
15 |
#define ORB1_DBG_CLR(color) orb1_set_color(color) |
|
16 |
#define ORB2_DBG_CLR(color) orb2_set_color(color) |
|
17 |
#else |
|
18 |
#define ORB1_DBG_CLR(color) |
|
19 |
#define ORB2_DBG_CLR(color) |
|
20 |
#endif |
|
21 |
|
|
22 |
#ifdef DEBUG_INTERSECTION |
|
23 |
#define DBG_USBS(str) usb_puts(str) |
|
24 |
#else |
|
25 |
#define DBG_USBS(str) |
|
26 |
#endif |
|
27 |
|
|
12 | 28 |
static int state, sign, turnDir; |
13 | 29 |
static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3; |
14 | 30 |
unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0; |
... | ... | |
33 | 49 |
|
34 | 50 |
id = get_robotid(); |
35 | 51 |
sign = 0; |
36 |
orb1_set_color(GREEN);
|
|
52 |
ORB1_DBG_CLR(GREEN);
|
|
37 | 53 |
delay_ms(1000); |
38 |
orb1_set_color(ORB_OFF);
|
|
54 |
ORB1_DBG_CLR(ORB_OFF);
|
|
39 | 55 |
|
40 | 56 |
//Test code |
41 | 57 |
state = SROAD; |
... | ... | |
43 | 59 |
sendBuffer[1] = id; |
44 | 60 |
|
45 | 61 |
/* |
46 |
doDrive(200);
|
|
62 |
doDrive(180);
|
|
47 | 63 |
turn(DOUBLE, ILEFT); |
48 | 64 |
*/ |
49 | 65 |
/* |
... | ... | |
52 | 68 |
*/ |
53 | 69 |
|
54 | 70 |
while (1) { |
55 |
/*DTM Finite State Machine*/
|
|
71 |
/*DTN Finite State Machine*/
|
|
56 | 72 |
switch(state){ |
57 | 73 |
case SROAD:/*Following a normal road*/ |
58 | 74 |
/* implement other road behaviors? |
59 | 75 |
* -tailgating |
60 | 76 |
*/ |
61 |
#ifdef ORB_INTERSECTION |
|
62 |
orb1_set_color(WHITE); |
|
63 |
orb2_set_color(ORB_OFF); |
|
64 |
#endif |
|
77 |
ORB1_DBG_CLR(WHITE); |
|
78 |
ORB2_DBG_CLR(ORB_OFF); |
|
65 | 79 |
start(); |
66 | 80 |
done = false; |
67 | 81 |
while(!done){ |
68 |
sign = doDrive(200);
|
|
82 |
sign = doDrive(180);
|
|
69 | 83 |
switch(sign){ |
70 | 84 |
case NORMAL: |
71 | 85 |
case FINISHED: |
... | ... | |
84 | 98 |
case SINTERSECTION_ENTER:/*Entering, and in intersection*/ |
85 | 99 |
stop(); |
86 | 100 |
doDrive(0); |
87 |
#ifdef ORB_INTERSECTION |
|
88 |
orb1_set_color(RED); |
|
89 |
orb2_set_color(ORB_OFF); |
|
90 |
#endif |
|
101 |
ORB1_DBG_CLR(RED); |
|
102 |
ORB2_DBG_CLR(ORB_OFF); |
|
91 | 103 |
|
92 |
#ifdef DEBUG_INTERSECTION |
|
93 |
usb_puts("STATE: SINTERSECTION_ENTER\n"); |
|
94 |
#endif |
|
104 |
DBG_USBS("STATE: SINTERSECTION_ENTER\n"); |
|
95 | 105 |
|
96 | 106 |
/*Intersection queue: |
97 | 107 |
*Each robot when entering the intersection will check for other robots |
... | ... | |
124 | 134 |
while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue |
125 | 135 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER); |
126 | 136 |
if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier... |
127 |
#ifdef ORB_INTERSECTION |
|
128 |
orb2_set_color(PURPLE); |
|
129 |
#endif |
|
137 |
ORB2_DBG_CLR(PURPLE); |
|
130 | 138 |
enterIntersection(); |
131 | 139 |
retried = 1; |
132 | 140 |
} |
133 | 141 |
switch (ret) { |
134 | 142 |
case KPLACEDINQUEUE: |
135 |
#ifdef ORB_INTERSECTION |
|
136 |
orb2_set_color(GREEN); |
|
137 |
#endif |
|
143 |
ORB2_DBG_CLR(GREEN); |
|
138 | 144 |
done = true; |
139 | 145 |
break; |
140 | 146 |
case KFAILEDTOQUEUE: |
141 |
#ifdef DEBUG_INTERSECTION |
|
142 |
usb_puts("Failed to queue\n"); |
|
143 |
#endif |
|
144 |
#ifdef ORB_INTERSECTION |
|
145 |
orb2_set_color(RED); |
|
146 |
#endif |
|
147 |
DBG_USBS("Failed to queue\n"); |
|
148 |
ORB2_DBG_CLR(RED); |
|
147 | 149 |
enterIntersection(); |
148 | 150 |
rtc_reset(); |
149 | 151 |
break; |
150 | 152 |
case KRESOLVINGENTER: |
151 | 153 |
|
152 |
#ifdef ORB_INTERSECTION |
|
153 |
orb2_set_color(ORANGE); |
|
154 |
#endif |
|
154 |
ORB2_DBG_CLR(ORANGE); |
|
155 | 155 |
state = SINTERSECTION_ENTER_RESOLV; |
156 | 156 |
done = true; |
157 | 157 |
break; |
... | ... | |
160 | 160 |
break; |
161 | 161 |
|
162 | 162 |
case SINTERSECTION_ENTER_RESOLV: |
163 |
#ifdef ORB_INTERSECTION |
|
164 |
orb1_set_color(PURPLE); |
|
165 |
orb2_set_color(ORB_OFF); |
|
166 |
#endif |
|
167 |
#ifdef DEBUG_INTERSECTION |
|
168 |
usb_puts("STATE: SINTERSECTION_ENTER_RESOLV\n"); |
|
169 |
#endif |
|
163 |
ORB1_DBG_CLR(PURPLE); |
|
164 |
ORB2_DBG_CLR(ORB_OFF); |
|
165 |
DBG_USBS("STATE: SINTERSECTION_ENTER_RESOLV\n"); |
|
170 | 166 |
|
171 | 167 |
rtc_reset(); |
172 | 168 |
done = false; |
... | ... | |
175 | 171 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV); |
176 | 172 |
switch (ret) { |
177 | 173 |
case KRESOLVINGENTER: |
178 |
#ifdef ORB_INTERSECTION |
|
179 |
orb2_set_color(YELLOW);
|
|
180 |
#endif |
|
174 |
|
|
175 |
ORB2_DBG_CLR(YELLOW);
|
|
176 |
|
|
181 | 177 |
break; |
182 | 178 |
case KPLACEDINQUEUE: |
183 |
#ifdef ORB_INTERSECTION |
|
184 |
orb2_set_color(GREEN);
|
|
185 |
#endif |
|
179 |
|
|
180 |
ORB2_DBG_CLR(GREEN);
|
|
181 |
|
|
186 | 182 |
done = true; |
187 | 183 |
break; |
188 | 184 |
case KFAILEDTOQUEUE: |
... | ... | |
218 | 214 |
state = SINTERSECTION_WAIT; |
219 | 215 |
break; |
220 | 216 |
case SINTERSECTION_WAIT:/*Waiting in intersection */ |
221 |
#ifdef ORB_INTERSECTION |
|
222 |
orb1_set_color(YELLOW); |
|
223 |
orb2_set_color(ORB_OFF); |
|
224 |
#endif |
|
225 |
#ifdef DEBUG_INTERSECTION |
|
226 |
usb_puts("STATE: SINTERSECTION_WAIT\n"); |
|
227 |
#endif |
|
228 | 217 |
|
218 |
ORB1_DBG_CLR(YELLOW); |
|
219 |
ORB2_DBG_CLR(ORB_OFF); |
|
220 |
|
|
221 |
DBG_USBS("STATE: SINTERSECTION_WAIT\n"); |
|
222 |
|
|
223 |
|
|
229 | 224 |
while(queuePrevBot != (char) -1){ |
230 | 225 |
int ret = wirelessPacketHandle(state); |
231 | 226 |
switch (ret){ |
232 | 227 |
case KFIRSTINQUEUE: |
233 |
#ifdef ORB_INTERSECTION |
|
234 |
orb2_set_color(GREEN);
|
|
235 |
#endif |
|
228 |
|
|
229 |
ORB2_DBG_CLR(GREEN);
|
|
230 |
|
|
236 | 231 |
state = SINTERSECTION_DRIVE; |
237 | 232 |
break; |
238 | 233 |
case KREPLIEDTOENTER: |
239 |
#ifdef ORB_INTERSECTION |
|
240 |
orb2_set_color(BLUE);
|
|
241 |
#endif |
|
234 |
|
|
235 |
ORB2_DBG_CLR(BLUE);
|
|
236 |
|
|
242 | 237 |
break; |
243 | 238 |
case KRESOLVINGENTER: |
244 |
#ifdef ORB_INTERSECTION |
|
245 |
orb2_set_color(ORANGE);
|
|
246 |
#endif |
|
239 |
|
|
240 |
ORB2_DBG_CLR(ORANGE);
|
|
241 |
|
|
247 | 242 |
break; |
248 | 243 |
} |
249 | 244 |
} |
... | ... | |
251 | 246 |
break; |
252 | 247 |
|
253 | 248 |
case SINTERSECTION_DRIVE: |
254 |
#ifdef DEBUG_INTERSECTION |
|
255 |
usb_puts("STATE: SINTERSECTION_DRIVE\n"); |
|
256 |
#endif |
|
257 |
#ifdef ORB_INTERSECTION |
|
258 |
orb1_set_color(GREEN); |
|
259 |
orb2_set_color(ORB_OFF); |
|
260 |
#endif |
|
249 |
DBG_USBS("STATE: SINTERSECTION_DRIVE\n"); |
|
250 |
|
|
251 |
|
|
252 |
ORB1_DBG_CLR(GREEN); |
|
253 |
ORB2_DBG_CLR(ORB_OFF); |
|
254 |
|
|
261 | 255 |
start(); |
262 | 256 |
turn(getIntersectType(sign), turnDir); |
263 |
while(doDrive(200) != FINISHED){
|
|
257 |
while(doDrive(180) != FINISHED){
|
|
264 | 258 |
//while(!button2_click()){ |
265 | 259 |
int ret = wirelessPacketHandle(state); |
266 | 260 |
switch (ret){ |
267 | 261 |
case KREPLIEDTOENTER: |
268 |
#ifdef ORB_INTERSECTION |
|
269 |
orb2_set_color(BLUE);
|
|
270 |
#endif |
|
262 |
|
|
263 |
ORB2_DBG_CLR(BLUE);
|
|
264 |
|
|
271 | 265 |
break; |
272 | 266 |
case KRESOLVINGENTER: |
273 |
#ifdef ORB_INTERSECTION |
|
274 |
orb2_set_color(ORANGE);
|
|
275 |
#endif |
|
267 |
|
|
268 |
ORB2_DBG_CLR(ORANGE);
|
|
269 |
|
|
276 | 270 |
break; |
277 | 271 |
} |
278 | 272 |
} |
279 | 273 |
|
280 | 274 |
//Exits intersection |
281 |
#ifdef ORB_INTERSECTION |
|
282 |
orb1_set_color(WHITE); |
|
283 |
#endif |
|
284 | 275 |
|
276 |
ORB1_DBG_CLR(WHITE); |
|
277 |
|
|
278 |
|
|
285 | 279 |
sendBuffer[0] = WINTERSECTIONEXIT; |
286 | 280 |
sendBuffer[2] = intersectionNum;//Intersection # |
287 | 281 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
... | ... | |
303 | 297 |
state = SROAD; |
304 | 298 |
break; |
305 | 299 |
case SHIGHWAY:/*On highway*/ |
306 |
#ifdef ORB_INTERSECTION |
|
307 |
orb1_set_color(CYAN);
|
|
308 |
#endif |
|
300 |
|
|
301 |
ORB1_DBG_CLR(CYAN);
|
|
302 |
|
|
309 | 303 |
while(!button1_click()){ |
310 | 304 |
highwayFSM(); |
311 | 305 |
} |
... | ... | |
517 | 511 |
sendBuffer[4] = turnDir; |
518 | 512 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
519 | 513 |
} |
514 |
|
|
520 | 515 |
#endif |
Also available in: Unified diff