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.
| 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