Revision 1984 trunk/code/projects/traffic_navigation/mapping.c

View differences:

mapping.c
1
#include "stdlib.h"
1 2
#include "mapping.h"
2 3
#include "lineDrive.h"
3 4
#include <dragonfly_lib.h>
......
10 11
 * after its creation, the graph is transmitted wirelessly */
11 12
node intersections[NUM_FEATURES]; 
12 13

  
14

  
15

  
16
/* This is run only once at the beginning of mapping when the robot is
17
 * placed somewhere randomly on the map in the middle of the road*/
18
char driveToNextInt(){
19
        char barcode; 
20
        
21
        do {
22
            barcode = (char) doDrive(200);
23
        } 
24
        while(barcode < 0); /* Condition codes are all neg. */
25

  
26
        return barcode;
27
}
28

  
29

  
30

  
13 31
/* 
14 32
 * Traverses the map using DFS 
15 33
 * Returns 0 if all of the intersections in the database were seen
16 34
 * 1 otherwise 
17 35
 */
18
int createEdge(edge* newEdge){
19
	char barcode;
20
	char time;
21
	rtc_init(SIXTEENTH_SECOND, NULL);
22
	rtc_reset();
23
    do
24
    {
25
		barcode = (char) doDrive(200);
26
    } while(barcode == NOBARCODE);
27
	time = rtc_get();
28
	newEdge->to = barcode;
29
	newEdge->dist = time;
30
	return 0;
36
char createEdge(edge* newEdge, int type, int direction){
37
        char barcode;
38
        char time;
39

  
40
        rtc_init(SIXTEENTH_SECOND, NULL);
41
        rtc_reset();
42

  
43
        turn(type, direction);
44

  
45
        do {
46
            barcode = (char) doDrive(200);
47
        }
48
        while(barcode < 0); /* Condition codes are all neg. */
49

  
50
        time = rtc_get();
51

  
52
        newEdge->to = barcode;
53
        newEdge->dist = time;
54
        
55
        return barcode;
31 56
}
32 57

  
58
/* This function performs mapping */
33 59
int createMap(){
34 60

  
35 61
    int seenout_count = 0;			/* The number of intersections we have completely seen */
36
    //int hist_count = 0;			/* The size of our history stack */ 
37 62

  
38 63
    char seen[NUM_FEATURES];		/* Have we seen this node? */
39 64
    char seenout[NUM_FEATURES];		/* Have we seen the outoging edges of this node? */
40 65

  
66
    int outEdges, currInt, chosenDir;
67

  
68
    initGraph(seen, seenout);
69

  
70
    /* Drives to the nearest intersection */
71
    char barcode = driveToNextInt();
72

  
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));
80

  
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

  
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
            }
94
        }
95
        /* Traverses edge, stores information in struct */
96
        createEdge(&(intersections[currInt].outgoingEdges[chosenDir]),
97
                    getIntersectType(currInt), chosenDir);
98
    }
99

  
100
/* We are done traversing send the graph to the robots */
101
sendIntersectionGraph();
102

  
103

  
104
return 0;
105
}
106

  
107
/* Initializes the elements in the graph to 0 */
108
void initGraph(char* seen, char* seenout){
109

  
41 110
    int i;
42
    int outEdges, currInt, chosenEdge;
43 111

  
44

  
45 112
    /* Initialize all of these arrays */
46 113
    for(i = 0; i<NUM_FEATURES; i++) {
47 114
            seen[i] = 0;
......
59 126
            intersections[i].outgoingEdges[3].to = 0;
60 127
            intersections[i].outgoingEdges[3].dist = 0;	
61 128
    }
129
}
62 130

  
63 131

  
64
        /* Drives to the nearest intersection */
65
        int barcode = nextInt();
66

  
67
        while(seenout_count < NUM_FEATURES)
68
        {
69
                currInt = getIntersectNum(barcode);
70
                seen[currInt] = 1;
71

  
72
                /* Get the number of outgoing edges */
73
                outEdges = getNumOut(getIntersectType(currInt));
74

  
75
                /* Randomly choose an outgoing edge that we have not seen to go down 
76
                 * if no such edge exists */
77
								// TODO: include the appropriate header file for rand()
78
                chosenEdge = rand() % outEdges;
79

  
80
                /* We have not seen all the outgoing edges */
81
                if(!seenout[currInt]){
82
                        intersections[currInt].outSeen++;
83
                        /* We have finished seeing all outgoing edges of the intersection */
84
                        if(intersections[currInt].numOut == intersections[currInt].outSeen){
85
                                seenout[currInt] = 1;
86
                                seenout_count ++;
87
                        }
88
                }
89
                /* Traverses edge, stores information in struct */
90
								/* TODO: Define traverseEdge, then uncomment this line
91
                traverseEdge(chosenEdge, &(intersections[currInt].outgoingEdges[chosenEdge])); */
92
        }
93

  
94
        /* We are done traversing send the graph to the robots */
95
        sendIntersectionGraph();
96

  
97

  
98
        return 0;
99
}
100

  
101 132
/* Given an intersection type, returns the number of outgoing edges */
102 133
int getNumOut(int type) {
103 134
    switch(type){
104
        case 0:
105
            return 0;
106
        case 1:
107
            return 1; 
108
        case 2:
109
            return 2; 
110
        case 3:
111
            return 3;
112
        case 4:
113
            return 4;
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;
114 140
    }
115

  
116 141
    return -1;
117 142
}
118 143

  
119

  
120
/* Creates an edge in the graph */
121
int insertEdge(){
122
	return 0;
123
}
124

  
125

  
126 144
/* 
127 145
 * Drives to the next intersection and returns its ID
128 146
 * If we are at a dead end, returns -1
129 147
 */
130 148
int nextInt(){
131
	return 0;
149
        return 0;
132 150
}
133 151

  
134 152

  
......
138 156
 *
139 157
 */
140 158
int encodeNode(node n){
141
	return 0;
159
        return 0;
142 160
}
143 161

  

Also available in: Unified diff