Revision 1896
Changed main and validTurns to get the test going. Should theoretically drive through the cloverleaf, switching between road and intersection mode, but has trouble following the line and keeps reading phantom barcodes.
| main.c | ||
|---|---|---|
| 8 | 8 |
#include "traffic_navigation.h" |
| 9 | 9 |
|
| 10 | 10 |
static int state, sign, dataLength, pingWaitTime; |
| 11 |
static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum; |
|
| 11 |
static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum, turnDir;
|
|
| 12 | 12 |
static unsigned char *packet; |
| 13 | 13 |
|
| 14 |
void enterIntersectionQueue(int sign); |
|
| 15 |
void waitInIntersectionQueue(); |
|
| 16 |
void driveThroughIntersection(int sign); |
|
| 17 |
|
|
| 14 | 18 |
int main (void) {
|
| 15 |
|
|
| 19 |
|
|
| 16 | 20 |
/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */ |
| 17 | 21 |
dragonfly_init(ALL_ON); |
| 18 | 22 |
xbee_init(); |
| ... | ... | |
| 21 | 25 |
rtc_init(SIXTEENTH_SECOND, NULL); |
| 22 | 26 |
wl_basic_init_default(); |
| 23 | 27 |
wl_set_channel(13); |
| 28 |
initializeData(); |
|
| 24 | 29 |
|
| 25 | 30 |
id = get_robotid(); |
| 26 | 31 |
sign = 0; |
| ... | ... | |
| 58 | 63 |
*/ |
| 59 | 64 |
prevBot = 0; |
| 60 | 65 |
nextBot = 0; |
| 61 |
enterIntersectionQueue(); |
|
| 62 | 66 |
|
| 67 |
sign = 14; //Test code until barcodes integrated |
|
| 68 |
|
|
| 69 |
enterIntersectionQueue(sign); |
|
| 70 |
|
|
| 63 | 71 |
orb1_set_color(PURPLE); |
| 64 | 72 |
//waits for its turn |
| 65 | 73 |
|
| ... | ... | |
| 67 | 75 |
|
| 68 | 76 |
orb1_set_color(RED); |
| 69 | 77 |
//Drives through intersection |
| 70 |
driveThroughIntersection(); |
|
| 78 |
driveThroughIntersection(sign);
|
|
| 71 | 79 |
|
| 72 | 80 |
//Exits intersection |
| 73 | 81 |
sendBuffer[0] = WINTERSECTIONEXIT; |
| ... | ... | |
| 79 | 87 |
sign = doDrive(255); |
| 80 | 88 |
if((sign >= 0) || button1_click()){
|
| 81 | 89 |
start(); |
| 82 |
if(1){
|
|
| 90 |
if(0){
|
|
| 83 | 91 |
state = SHIGHWAY; |
| 84 | 92 |
break; |
| 85 | 93 |
}else{
|
| ... | ... | |
| 104 | 112 |
|
| 105 | 113 |
} |
| 106 | 114 |
|
| 107 |
void enterIntersectionQueue(){
|
|
| 115 |
void enterIntersectionQueue(int sign){
|
|
| 116 |
|
|
| 117 |
//Choose turn direction |
|
| 118 |
intersectionNum = getIntersectType(sign); |
|
| 119 |
turnDir = validateTurn(sign, getTurnType(14)); |
|
| 120 |
|
|
| 108 | 121 |
//Sends packet announcing its entry to the intersection |
| 109 | 122 |
sendBuffer[0] = WINTERSECTIONENTRY; |
| 110 | 123 |
sendBuffer[2] = intersectionNum;//Intersection # |
| 111 |
sendBuffer[3] = 0;//From Direction
|
|
| 112 |
sendBuffer[4] = 2;//To Direction
|
|
| 124 |
sendBuffer[3] = getIntersectPos(sign);
|
|
| 125 |
sendBuffer[4] = turnDir;
|
|
| 113 | 126 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
| 114 | 127 |
orb1_set_color(BLUE); |
| 115 | 128 |
stop(); |
| ... | ... | |
| 127 | 140 |
break; |
| 128 | 141 |
}else if(packet[1] != id){//Someone else got here first, try again
|
| 129 | 142 |
sendBuffer[0] = WINTERSECTIONENTRY; |
| 130 |
sendBuffer[2] = intersectionNum;//Intersection #
|
|
| 131 |
sendBuffer[3] = 0;//From Direction
|
|
| 132 |
sendBuffer[4] = 2;//To Direction
|
|
| 143 |
sendBuffer[2] = intersectionNum; |
|
| 144 |
sendBuffer[3] = getIntersectPos(sign);
|
|
| 145 |
sendBuffer[4] = turnDir;
|
|
| 133 | 146 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
| 134 | 147 |
orb2_set_color(ORANGE); |
| 135 | 148 |
rtc_reset(); |
| ... | ... | |
| 208 | 221 |
} |
| 209 | 222 |
} |
| 210 | 223 |
|
| 211 |
void driveThroughIntersection(){
|
|
| 224 |
void driveThroughIntersection(int sign){
|
|
| 212 | 225 |
//Code to choose path through intersection goes in this while loop |
| 213 | 226 |
//But here's the code to handle wireless while in the intersection... |
| 214 | 227 |
start(); |
| 215 |
turn(DOUBLE, IRIGHT);//Change paramers to variables
|
|
| 228 |
turn(getIntersectType(sign), turnDir);
|
|
| 216 | 229 |
while(doDrive(255) != FINISHED){
|
| 217 | 230 |
dataLength = 0; |
| 218 | 231 |
packet = NULL; |
Also available in: Unified diff