Revision 1972
Traffic Navigation:
Fixed some various issues with the new main (bad calls & added barcode error
handling)
It compiles by default now.
| 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 |
Also available in: Unified diff