Revision 1972
Traffic Navigation:
Fixed some various issues with the new main (bad calls & added barcode error
handling)
It compiles by default now.
trunk/code/projects/traffic_navigation/traffic_navigation.h | ||
---|---|---|
1 | 1 |
#ifndef TRAFFIC_NAVIGATION_H |
2 | 2 |
|
3 |
/* Debug Options - These must go before includes */ |
|
4 |
#define MAIN_NEW /* compile the new main */ |
|
5 |
#define ORB_INTERSECTION |
|
6 |
|
|
3 | 7 |
#include <dragonfly_lib.h> |
4 | 8 |
#include <wl_basic.h> |
5 | 9 |
#include "../linefollowing/lineDrive.h" |
... | ... | |
7 | 11 |
#include "intersectData.h" |
8 | 12 |
#include "validTurns.h" |
9 | 13 |
|
10 |
/* Debug Options */ |
|
11 |
#ifdef DEBUGALL |
|
12 |
#define ORB_HIGHWAY |
|
13 |
#define ORB_LINEFOLLOW |
|
14 |
#define ORB_INTERSECTON |
|
15 |
#define DEBUG_CA |
|
16 |
#define DEBUG_INTERSECTION |
|
17 |
#endif |
|
18 |
|
|
19 | 14 |
/*States*/ |
20 | 15 |
#define SROAD 0 |
21 | 16 |
#define SINTERSECTION 9 /* for old version */ |
trunk/code/projects/traffic_navigation/main-new.c | ||
---|---|---|
1 |
#ifdef MAIN_NEW |
|
2 | 1 |
/* |
3 | 2 |
* main.c for Traffic Navigation |
4 | 3 |
* Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG |
... | ... | |
8 | 7 |
|
9 | 8 |
#include "traffic_navigation.h" |
10 | 9 |
#include "../linefollowing/lineDrive.h" |
10 |
#ifdef MAIN_NEW |
|
11 | 11 |
|
12 | 12 |
static int state, sign, turnDir; |
13 | 13 |
static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3; |
14 | 14 |
unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0; |
15 |
bool done; |
|
15 | 16 |
|
16 | 17 |
int wirelessPacketHandle(int state); |
17 | 18 |
void enterIntersection(void); |
... | ... | |
32 | 33 |
|
33 | 34 |
id = get_robotid(); |
34 | 35 |
sign = 0; |
36 |
orb1_set_color(GREEN); |
|
37 |
delay_ms(1000); |
|
38 |
orb1_set_color(ORB_OFF); |
|
35 | 39 |
|
36 | 40 |
//Test code |
37 | 41 |
state = SROAD; |
... | ... | |
54 | 58 |
/* implement other road behaviors? |
55 | 59 |
* -tailgating |
56 | 60 |
*/ |
57 |
//read barcode |
|
58 |
sign = doDrive(200); |
|
59 | 61 |
#ifdef ORB_INTERSECTION |
60 | 62 |
orb1_set_color(ORB_OFF); |
61 | 63 |
orb2_set_color(ORB_OFF); |
62 | 64 |
#endif |
63 |
if(button1_click()){ |
|
64 |
state = SINTERSECTION_ENTER; |
|
65 |
done = false; |
|
66 |
while(!done){ |
|
67 |
sign = doDrive(200); |
|
68 |
switch(sign){ |
|
69 |
case NORMAL: |
|
70 |
case FINISHED: |
|
71 |
case LOST: |
|
72 |
case ERROR: |
|
73 |
break; |
|
74 |
default: |
|
75 |
//we have a barcode! |
|
76 |
state = SINTERSECTION_ENTER; |
|
77 |
done = true; |
|
78 |
break; |
|
79 |
|
|
80 |
} |
|
65 | 81 |
} |
66 | 82 |
break; |
67 | 83 |
case SINTERSECTION_ENTER:/*Entering, and in intersection*/ |
... | ... | |
87 | 103 |
resolvPrevBotDraw = 0; //the random priority number that bot has |
88 | 104 |
resolvDraw = 0; //my random priority number |
89 | 105 |
|
90 |
sign = 0; //Test code until barcodes integrated |
|
91 |
intersectionNum = getIntersectType(sign); |
|
92 |
turnDir = validateTurn(sign, getTurnType(4)); |
|
106 |
intersectionNum = getIntersectNum(sign); |
|
107 |
if(intersectionNum == (char) -1){ //invalid |
|
108 |
state = SROAD; |
|
109 |
break; |
|
110 |
} |
|
111 |
turnDir = validateTurn(sign, getTurnType(sign)); |
|
112 |
if(turnDir == (char) -1){ //invalid |
|
113 |
state = SROAD; |
|
114 |
break; |
|
115 |
} |
|
93 | 116 |
|
94 | 117 |
enterIntersection(); //sends wireless packet for entry |
95 | 118 |
state = SINTERSECTION_WAIT; |
96 | 119 |
|
97 | 120 |
rtc_reset(); //reset rtc for timeout wait for reply |
98 |
bool done = false;
|
|
121 |
done = false; |
|
99 | 122 |
char retried = 0; |
100 | 123 |
while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue |
101 | 124 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER); |
... | ... | |
235 | 258 |
orb2_set_color(ORB_OFF); |
236 | 259 |
#endif |
237 | 260 |
start(); |
238 |
turn(getIntersectType(0), turnDir);
|
|
261 |
turn(getIntersectType(sign), turnDir);
|
|
239 | 262 |
while(doDrive(200) != FINISHED){ |
240 | 263 |
//while(!button2_click()){ |
241 | 264 |
int ret = wirelessPacketHandle(state); |
... | ... | |
263 | 286 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
264 | 287 |
|
265 | 288 |
//Exits intersection |
289 |
/* |
|
266 | 290 |
while(1){ |
267 | 291 |
if(button1_click()){ |
268 | 292 |
start(); |
... | ... | |
274 | 298 |
state = SROAD; |
275 | 299 |
break; |
276 | 300 |
} |
277 |
} |
|
301 |
}*/ |
|
302 |
state = SROAD; |
|
278 | 303 |
break; |
279 | 304 |
case SHIGHWAY:/*On highway*/ |
280 | 305 |
#ifdef ORB_INTERSECTION |
trunk/code/projects/traffic_navigation/main.c | ||
---|---|---|
7 | 7 |
|
8 | 8 |
#include "traffic_navigation.h" |
9 | 9 |
#include "../linefollowing/lineDrive.h" |
10 |
#ifndef MAIN_NEW |
|
10 | 11 |
|
11 | 12 |
static int state, sign, dataLength, pingWaitTime, turnDir; |
12 | 13 |
static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum; |
... | ... | |
265 | 266 |
} |
266 | 267 |
} |
267 | 268 |
} |
269 |
#endif |
Also available in: Unified diff