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.
| trunk/code/projects/traffic_navigation/intersectData.h | ||
|---|---|---|
| 82 | 82 |
// Global variable for the intersection database. You can change the size here. Size == number of intersections. |
| 83 | 83 |
int IntersectData[1]; |
| 84 | 84 |
|
| 85 |
void initializeData(); |
|
| 85 | 86 |
void insertIntersection(int barcode, int intersect_type, int intersect_position); |
| 86 | 87 |
|
| 87 | 88 |
// Functions to get data from the Database: |
| trunk/code/projects/traffic_navigation/validTurns.h | ||
|---|---|---|
| 56 | 56 |
*****************************/ |
| 57 | 57 |
|
| 58 | 58 |
int validateTurn(int barcode, int turn_type); |
| 59 |
int getTurnType(int barcode); |
|
| 59 | 60 |
|
| 60 | 61 |
#endif |
| trunk/code/projects/traffic_navigation/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; |
| trunk/code/projects/traffic_navigation/validTurns-test.c | ||
|---|---|---|
| 2 | 2 |
#include "intersectData.h" |
| 3 | 3 |
#include "validTurns.h" |
| 4 | 4 |
|
| 5 |
#define TEST_VALIDTURNS |
|
| 5 |
//#define TEST_VALIDTURNS
|
|
| 6 | 6 |
|
| 7 | 7 |
#ifdef TEST_VALIDTURNS |
| 8 | 8 |
|
| trunk/code/projects/traffic_navigation/validTurns.c | ||
|---|---|---|
| 7 | 7 |
* |
| 8 | 8 |
*/ |
| 9 | 9 |
|
| 10 |
#include <dragonfly_lib.h> |
|
| 10 | 11 |
|
| 11 |
/******************************Random Num Gen Version of validTurns |
|
| 12 |
/******************************Random Num Gen Version of validTurns*/
|
|
| 12 | 13 |
int randomNumGen(int max){
|
| 13 | 14 |
int x = range_read_distance(IR2); |
| 14 | 15 |
if (x>0) return range_read_distance(IR2)%max; |
| 15 | 16 |
else return randomNumGen(max); |
| 16 | 17 |
} |
| 17 |
|
|
| 18 |
/* |
|
| 18 | 19 |
int getIntersectType(int barcode) |
| 19 | 20 |
{
|
| 20 | 21 |
int x = randomNumGen(4); |
| ... | ... | |
| 30 | 31 |
{
|
| 31 | 32 |
return randomNumGen(max); |
| 32 | 33 |
} |
| 33 |
|
|
| 34 |
*/ |
|
| 34 | 35 |
int getTurnType(int barcode) |
| 35 | 36 |
{
|
| 36 | 37 |
return randomNumGen(4); |
| 37 | 38 |
} |
| 38 | 39 |
|
| 39 |
*/ |
|
| 40 | 40 |
|
| 41 | 41 |
#include "intersectData.h" |
| 42 | 42 |
#include "validTurns.h" |
Also available in: Unified diff