Revision 1999
Refactored main_new timings so that bots travel through intersections more
fluidly
| trunk/code/projects/traffic_navigation/main-intersectionDebug.c | ||
|---|---|---|
| 95 | 95 |
|
| 96 | 96 |
bool done = false; |
| 97 | 97 |
bool retried = false; |
| 98 |
while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue
|
|
| 98 |
while(rtc_get() < 8 && !done){//waits for a reply, otherwise assumes it is first in queue
|
|
| 99 | 99 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER); |
| 100 |
if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier...
|
|
| 100 |
if(rtc_get() > 6 && !retried){//by now all resolvs should be done from bots that arrived earlier...
|
|
| 101 | 101 |
orb2_set_color(PURPLE); |
| 102 | 102 |
enterIntersection(); |
| 103 | 103 |
retried = true; |
| ... | ... | |
| 130 | 130 |
|
| 131 | 131 |
done = false; |
| 132 | 132 |
retried = false; |
| 133 |
while(rtc_get() < 9 && !done){
|
|
| 133 |
while(rtc_get() < 4 && !done){
|
|
| 134 | 134 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV); |
| 135 | 135 |
switch (ret) {
|
| 136 | 136 |
case KRESOLVINGENTER: |
| ... | ... | |
| 151 | 151 |
enterIntersection(); |
| 152 | 152 |
rtc_reset(); |
| 153 | 153 |
retried = true; |
| 154 |
} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 6){
|
|
| 154 |
} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 2){
|
|
| 155 | 155 |
//send a intersection reply to myself to |
| 156 | 156 |
//trigger other bots to enter queue. |
| 157 | 157 |
sendBuffer[0] = WINTERSECTIONREPLY; |
| ... | ... | |
| 170 | 170 |
orb2_set_color(ORB_OFF); |
| 171 | 171 |
usb_puts("STATE: SINTERSECTION_WAIT\n");
|
| 172 | 172 |
|
| 173 |
done = false; |
|
| 173 | 174 |
while(queuePrevBot != (char) -1){
|
| 175 |
done = true; |
|
| 174 | 176 |
int ret = wirelessPacketHandle(state); |
| 175 | 177 |
switch (ret){
|
| 176 | 178 |
case KFIRSTINQUEUE: |
| ... | ... | |
| 185 | 187 |
break; |
| 186 | 188 |
} |
| 187 | 189 |
} |
| 190 |
//hack to make sure bot that just left intersection is |
|
| 191 |
//really out of the intersection. |
|
| 192 |
rtc_reset(); |
|
| 193 |
while(rtc_get() < 2 && done){//wait one second
|
|
| 194 |
wirelessPacketHandle(state); |
|
| 195 |
} |
|
| 196 |
|
|
| 188 | 197 |
state = SINTERSECTION_DRIVE; |
| 189 | 198 |
break; |
| 199 |
state = SINTERSECTION_DRIVE; |
|
| 200 |
break; |
|
| 190 | 201 |
case SINTERSECTION_DRIVE: |
| 191 | 202 |
usb_puts("STATE: SINTERSECTION_DRIVE\n");
|
| 192 | 203 |
orb1_set_color(GREEN); |
| trunk/code/projects/traffic_navigation/main-new.c | ||
|---|---|---|
| 1 |
#define MAIN_NEW |
|
| 1 | 2 |
/* |
| 2 | 3 |
* main.c for Traffic Navigation |
| 3 | 4 |
* Runs the highest level behavior for the Dynamic Traffic Navigation (DTN) SURG |
| ... | ... | |
| 5 | 6 |
* Author: Colony Project, CMU Robotics Club |
| 6 | 7 |
*/ |
| 7 | 8 |
|
| 8 |
#define MAIN_NEW |
|
| 9 |
|
|
| 10 | 9 |
#include "traffic_navigation.h" |
| 11 | 10 |
#include "../linefollowing/lineDrive.h" |
| 12 | 11 |
#define ORB_INTERSECTION |
| ... | ... | |
| 138 | 137 |
rtc_reset(); //reset rtc for timeout wait for reply |
| 139 | 138 |
done = false; |
| 140 | 139 |
char retried = 0; |
| 141 |
while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue
|
|
| 140 |
while(rtc_get() < 8 && !done){//waits for a reply, otherwise assumes it is first in queue
|
|
| 142 | 141 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER); |
| 143 |
if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier...
|
|
| 142 |
if(rtc_get() > 6 && !retried){//by now all resolvs should be done from bots that arrived earlier...
|
|
| 144 | 143 |
ORB2_DBG_CLR(PURPLE); |
| 145 | 144 |
enterIntersection(); |
| 146 | 145 |
retried = 1; |
| ... | ... | |
| 174 | 173 |
rtc_reset(); |
| 175 | 174 |
done = false; |
| 176 | 175 |
retried = 0; |
| 177 |
while(rtc_get() < 9 && !done){
|
|
| 176 |
while(rtc_get() < 4 && !done){
|
|
| 178 | 177 |
int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV); |
| 179 | 178 |
switch (ret) {
|
| 180 | 179 |
case KRESOLVINGENTER: |
| ... | ... | |
| 206 | 205 |
//resolving but never have seen a bot with lower |
| 207 | 206 |
//priority than us. after the 6/16ths sec |
| 208 | 207 |
//timeout, assume we are first. |
| 209 |
} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 6){
|
|
| 208 |
} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 2){
|
|
| 210 | 209 |
//send a intersection reply to myself to |
| 211 | 210 |
//trigger other bots to enter queue. |
| 212 | 211 |
sendBuffer[0] = WINTERSECTIONREPLY; |
| ... | ... | |
| 253 | 252 |
//hack to make sure bot that just left intersection is |
| 254 | 253 |
//really out of the intersection. |
| 255 | 254 |
rtc_reset(); |
| 256 |
while(rtc_get() < 4 && done){//wait one second
|
|
| 255 |
while(rtc_get() < 2 && done){//wait one second
|
|
| 257 | 256 |
wirelessPacketHandle(state); |
| 258 | 257 |
} |
| 259 | 258 |
|
Also available in: Unified diff