Revision 1987
Updated mapping code. Fixed some bugs.
Usure about something - see the TODO. Will fix soon.
| trunk/code/projects/traffic_navigation/mapping.c | ||
|---|---|---|
| 17 | 17 |
* placed somewhere randomly on the map in the middle of the road*/ |
| 18 | 18 |
char driveToNextInt(){
|
| 19 | 19 |
char barcode; |
| 20 |
|
|
| 20 |
|
|
| 21 |
/* Keep driving until we see a barcode */ |
|
| 21 | 22 |
do {
|
| 22 |
barcode = (char) doDrive(200); |
|
| 23 |
barcode = (char) doDrive(200);
|
|
| 23 | 24 |
} |
| 24 | 25 |
while(barcode < 0); /* Condition codes are all neg. */ |
| 25 | 26 |
|
| ... | ... | |
| 42 | 43 |
|
| 43 | 44 |
turn(type, direction); |
| 44 | 45 |
|
| 46 |
/* Keep driving until we see a barcode */ |
|
| 45 | 47 |
do {
|
| 46 |
barcode = (char) doDrive(200); |
|
| 48 |
barcode = (char) doDrive(200);
|
|
| 47 | 49 |
} |
| 48 | 50 |
while(barcode < 0); /* Condition codes are all neg. */ |
| 49 | 51 |
|
| ... | ... | |
| 51 | 53 |
|
| 52 | 54 |
newEdge->to = barcode; |
| 53 | 55 |
newEdge->dist = time; |
| 54 |
|
|
| 56 |
|
|
| 55 | 57 |
return barcode; |
| 56 | 58 |
} |
| 57 | 59 |
|
| 58 | 60 |
/* This function performs mapping */ |
| 59 | 61 |
int createMap(){
|
| 60 | 62 |
|
| 61 |
int seenout_count = 0; /* The number of intersections we have completely seen */ |
|
| 63 |
char seen[NUM_FEATURES]; /* Have we been at this intersection before*/ |
|
| 64 |
char seenout[NUM_FEATURES]; /* Have we seen the outoging edges of this intersection? */ |
|
| 62 | 65 |
|
| 63 |
char seen[NUM_FEATURES]; /* Have we seen this node? */
|
|
| 64 |
char seenout[NUM_FEATURES]; /* Have we seen the outoging edges of this node? */
|
|
| 66 |
char seenall[NUM_FEATURES]; /* This is how the array should look like once we have
|
|
| 67 |
* seen everything */
|
|
| 65 | 68 |
|
| 66 |
int outEdges, currInt, chosenDir;
|
|
| 69 |
int outEdges, currInt, chosenDir, i;
|
|
| 67 | 70 |
|
| 68 |
initGraph(seen, seenout); |
|
| 71 |
/* Initialize the graph to all zeros */ |
|
| 72 |
initGraph(seen, seenout); |
|
| 69 | 73 |
|
| 70 |
/* Drives to the nearest intersection */ |
|
| 71 |
char barcode = driveToNextInt(); |
|
| 74 |
/* When we see all the outgoing edges for the intersections, |
|
| 75 |
* seenout will look as follows */ |
|
| 76 |
for(i=0; i<NUM_FEATURES; i++) |
|
| 77 |
seenall[i] = 1; |
|
| 72 | 78 |
|
| 73 |
while(seenout_count < NUM_FEATURES) |
|
| 74 |
{
|
|
| 75 |
currInt = getIntersectNum(barcode); |
|
| 76 |
seen[currInt] = 1; |
|
| 77 |
|
|
| 78 |
/* Get the number of outgoing edges */ |
|
| 79 |
outEdges = getNumOut(getIntersectType(currInt)); |
|
| 79 |
/* First drives to the nearest intersection */ |
|
| 80 |
char barcode = driveToNextInt(); |
|
| 80 | 81 |
|
| 81 |
/* Randomly choose an outgoing edge that we have not seen to go down |
|
| 82 |
* if no such edge exists */ |
|
| 83 |
// TODO: include the appropriate header file for rand() |
|
| 84 |
chosenDir = rand() % outEdges; |
|
| 85 | 82 |
|
| 86 |
/* We have not seen all the outgoing edges */ |
|
| 87 |
if(!seenout[currInt]){
|
|
| 88 |
intersections[currInt].outSeen++; |
|
| 89 |
/* We have finished seeing all outgoing edges of the intersection */ |
|
| 90 |
if(intersections[currInt].numOut == intersections[currInt].outSeen){
|
|
| 91 |
seenout[currInt] = 1; |
|
| 92 |
seenout_count ++; |
|
| 93 |
} |
|
| 83 |
/* Randomly traverses the graph until all edges are seen */ |
|
| 84 |
while(seenout != seenall) |
|
| 85 |
{
|
|
| 86 |
currInt = getIntersectNum(barcode); |
|
| 87 |
seen[currInt] = 1; |
|
| 88 |
|
|
| 89 |
/* Get the number of outgoing edges */ |
|
| 90 |
outEdges = getNumOut(getIntersectType(currInt)); |
|
| 91 |
|
|
| 92 |
/* Assign the number of outgoing edges for the current int */ |
|
| 93 |
intersections[currInt].numOut = outEdges; |
|
| 94 |
|
|
| 95 |
/* Randomly choose an outgoing edge that we have not seen to go down |
|
| 96 |
* if no such edge exists */ |
|
| 97 |
chosenDir = rand() % outEdges; |
|
| 98 |
|
|
| 99 |
/* We have not seen all the outgoing edges for the intersection we are at */ |
|
| 100 |
if(!seenout[currInt]){
|
|
| 101 |
intersections[currInt].outSeen++; |
|
| 102 |
/* We have finished seeing all outgoing edges of the intersection */ |
|
| 103 |
if(outEdges == intersections[currInt].outSeen){
|
|
| 104 |
seenout[currInt] = 1; |
|
| 105 |
} |
|
| 106 |
} |
|
| 107 |
|
|
| 108 |
/* Traverses edge, stores information in struct */ |
|
| 109 |
//TODO: Does the chosendDir, correspond with what is in Priya's Code? |
|
| 110 |
//A number between 0 and the number of outgoing edges ? |
|
| 111 |
createEdge(&(intersections[currInt].outgoingEdges[chosenDir]), |
|
| 112 |
getIntersectType(currInt), chosenDir); |
|
| 94 | 113 |
} |
| 95 |
/* Traverses edge, stores information in struct */ |
|
| 96 |
createEdge(&(intersections[currInt].outgoingEdges[chosenDir]), |
|
| 97 |
getIntersectType(currInt), chosenDir); |
|
| 98 |
} |
|
| 99 | 114 |
|
| 100 |
/* We are done traversing send the graph to the robots */ |
|
| 101 |
sendIntersectionGraph(); |
|
| 115 |
/* We are done traversing send the graph to the robots */
|
|
| 116 |
while(1) sendIntersectionGraph();
|
|
| 102 | 117 |
|
| 103 |
|
|
| 104 |
return 0; |
|
| 118 |
return 0; |
|
| 105 | 119 |
} |
| 106 | 120 |
|
| 107 | 121 |
/* Initializes the elements in the graph to 0 */ |
| 108 | 122 |
void initGraph(char* seen, char* seenout){
|
| 109 | 123 |
|
| 110 |
int i;
|
|
| 124 |
int i, j;
|
|
| 111 | 125 |
|
| 112 |
/* Initialize all of these arrays */ |
|
| 113 |
for(i = 0; i<NUM_FEATURES; i++) {
|
|
| 114 |
seen[i] = 0; |
|
| 115 |
seenout[i] = 0; |
|
| 116 |
intersections[i].type = 0; |
|
| 117 |
intersections[i].intNum = 0; |
|
| 118 |
intersections[i].numOut = 0; |
|
| 119 |
intersections[i].outSeen = 0; |
|
| 120 |
intersections[i].outgoingEdges[0].to = 0; |
|
| 121 |
intersections[i].outgoingEdges[0].dist = 0; |
|
| 122 |
intersections[i].outgoingEdges[1].to = 0; |
|
| 123 |
intersections[i].outgoingEdges[1].dist = 0; |
|
| 124 |
intersections[i].outgoingEdges[2].to = 0; |
|
| 125 |
intersections[i].outgoingEdges[2].dist = 0; |
|
| 126 |
intersections[i].outgoingEdges[3].to = 0; |
|
| 127 |
intersections[i].outgoingEdges[3].dist = 0; |
|
| 128 |
} |
|
| 126 |
/* Set all graph storge elements to 0 */ |
|
| 127 |
for(i = 0; i<NUM_FEATURES; i++) {
|
|
| 128 |
seen[i] = 0; |
|
| 129 |
seenout[i] = 0; |
|
| 130 |
intersections[i].type = 0; |
|
| 131 |
intersections[i].intNum = 0; |
|
| 132 |
intersections[i].numOut = 0; |
|
| 133 |
intersections[i].outSeen = 0; |
|
| 134 |
for(j = 0; j<4; j++){
|
|
| 135 |
intersections[i].outgoingEdges[0].to = 0; |
|
| 136 |
intersections[i].outgoingEdges[0].dist = 0; |
|
| 137 |
} |
|
| 138 |
} |
|
| 129 | 139 |
} |
| 130 | 140 |
|
| 131 | 141 |
|
| 132 | 142 |
/* Given an intersection type, returns the number of outgoing edges */ |
| 133 | 143 |
int getNumOut(int type) {
|
| 134 |
switch(type){
|
|
| 135 |
case 0: return 3; |
|
| 136 |
case 1: return -1; |
|
| 137 |
case 2: return -1; |
|
| 138 |
case 3: return 3; |
|
| 139 |
case 4: return 2; |
|
| 140 |
} |
|
| 141 |
return -1; |
|
| 144 |
switch(type){
|
|
| 145 |
case 0: return 3;
|
|
| 146 |
case 1: return -1;
|
|
| 147 |
case 2: return -1;
|
|
| 148 |
case 3: return 3;
|
|
| 149 |
case 4: return 2;
|
|
| 150 |
}
|
|
| 151 |
return -1;
|
|
| 142 | 152 |
} |
| 143 |
|
|
| 144 |
/* |
|
| 145 |
* Drives to the next intersection and returns its ID |
|
| 146 |
* If we are at a dead end, returns -1 |
|
| 147 |
*/ |
|
| 148 |
int nextInt(){
|
|
| 149 |
return 0; |
|
| 150 |
} |
|
| 151 |
|
|
| 152 |
|
|
| 153 |
/* |
|
| 154 |
* Given an intersection node returns an integer that |
|
| 155 |
* can be sent wirelessly |
|
| 156 |
* |
|
| 157 |
*/ |
|
| 158 |
int encodeNode(node n){
|
|
| 159 |
return 0; |
|
| 160 |
} |
|
| 161 |
|
|
Also available in: Unified diff