Statistics
| Revision:

root / trunk / code / projects / traffic_navigation / mapping.c @ 1984

History | View | Annotate | Download (4.15 KB)

1
#include "stdlib.h"
2
#include "mapping.h"
3
#include "lineDrive.h"
4
#include <dragonfly_lib.h>
5
#include <wl_basic.h>
6

    
7
//The last intersection that we encountered
8
int lastInt;
9

    
10
/* This array holds all of the intersections that are represented in the graph
11
 * after its creation, the graph is transmitted wirelessly */
12
node intersections[NUM_FEATURES]; 
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

    
31
/* 
32
 * Traverses the map using DFS 
33
 * Returns 0 if all of the intersections in the database were seen
34
 * 1 otherwise 
35
 */
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;
56
}
57

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

    
61
    int seenout_count = 0;                        /* The number of intersections we have completely seen */
62

    
63
    char seen[NUM_FEATURES];                /* Have we seen this node? */
64
    char seenout[NUM_FEATURES];                /* Have we seen the outoging edges of this node? */
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

    
110
    int i;
111

    
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
    }
129
}
130

    
131

    
132
/* Given an intersection type, returns the number of outgoing edges */
133
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;
142
}
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