Revision 1966
Added test map data encoding to intersectData.c. Fixe a whole bunch of stuff so
that at least this traffic_navigation compiles without warnings. Symlinked to
needed files in linefollow. Changed function declaration style. Added
delclaration of read_line to analog.h. You're welcome. Fixed several #includes.
You're all welcome.
intersectData.c | ||
---|---|---|
1 | 1 |
#include "intersectData.h" |
2 |
#include "lineDrive.h" |
|
2 |
#include "../linefollowing/lineDrive.h"
|
|
3 | 3 |
|
4 |
int NodeData[NUM_FEATURES]; |
|
5 |
|
|
4 | 6 |
/* |
5 | 7 |
* This function serves to make the code for the intersection database |
6 | 8 |
* as reuseable as possible. This part creates the implementation of the |
... | ... | |
10 | 12 |
|
11 | 13 |
// Does nothing if your indmax is out of bounds. |
12 | 14 |
void insertNode(int key, int featureid, int intersect_num, int node_type, int node_info){ |
13 |
if(key < NUM_FEATURES)
|
|
14 |
NodeData[key] = (featureid<<12) + (intersect_num<<5) + (node_type<<2) + node_info;
|
|
15 |
return;
|
|
15 |
if(key < NUM_FEATURES)
|
|
16 |
NodeData[key] = (featureid<<12) + (intersect_num<<5) + (node_type<<2) + node_info;
|
|
17 |
return;
|
|
16 | 18 |
} |
17 | 19 |
|
18 | 20 |
void insertIntersection(int key, int intersect_num, int node_type, int intersection_position){ |
19 |
if(key < NUM_FEATURES)
|
|
20 |
NodeData[key] = (INTERSECTION_NODE<<12) + (intersect_num<<5) + (node_type<<2)
|
|
21 |
+ intersection_position;
|
|
22 |
return;
|
|
21 |
if(key < NUM_FEATURES)
|
|
22 |
NodeData[key] = (INTERSECTION_NODE<<12) + (intersect_num<<5) + (node_type<<2)
|
|
23 |
+ intersection_position;
|
|
24 |
return;
|
|
23 | 25 |
} |
24 | 26 |
|
25 | 27 |
void insertRoad(int key, int intersect_num, int node_type, int road_type){ |
26 |
if(key < NUM_FEATURES)
|
|
27 |
NodeData[key] = (ROAD_NODE<<12) + (intersect_num<<5) + (node_type<<2) + road_type;
|
|
28 |
return;
|
|
28 |
if(key < NUM_FEATURES)
|
|
29 |
NodeData[key] = (ROAD_NODE<<12) + (intersect_num<<5) + (node_type<<2) + road_type;
|
|
30 |
return;
|
|
29 | 31 |
} |
30 | 32 |
|
31 | 33 |
/* |
... | ... | |
34 | 36 |
* only one that needs to be rewritten for ech map. |
35 | 37 |
* |
36 | 38 |
*/ |
37 |
/**********************************************************************NEEDS TO BE FIXED*/ |
|
38 | 39 |
void initializeData(){ |
39 |
//insert all intersections that are in the map. |
|
40 |
//THIS DATA FOR THE DEMO MAP |
|
41 |
/* insertNode(0, INTERSECTION_NODE, DOUBLE_C, TMIDDLE); |
|
42 |
insertNode(1, ROAD_NODE, ROAD, ROAD_SINGLE); |
|
43 |
insertNode(2, INTERSECTION_NODE, DOUBLE_C, TMIDDLE); |
|
44 |
insertNode(4,ROAD_NODE,ROAD,HIGHWAY); |
|
45 |
insertNode(5,INTERSECTION_NODE,DOUBLE_C,TMIDDLE); |
|
46 |
insertNode(8,ROAD_NODE,ROAD,ROAD_SINGLE); |
|
47 |
insertNode(10,ROAD_NODE,ROAD,ROAD_SINGLE); |
|
48 |
insertNode(16,INTERSECTION_NODE,DOUBLE_C, TMIDDLE); |
|
49 |
*/ return; |
|
40 |
//insert all intersections that are in the map. |
|
41 |
//THIS DATA FOR THE DEMO MAP |
|
42 |
insertIntersection(0, 1, SINGLE, SACROSS); |
|
43 |
insertIntersection(1, 1, SINGLE, SUP); |
|
44 |
insertIntersection(2, 2, SINGLE, SUP); |
|
45 |
insertIntersection(3, 2, SINGLE, SACROSS); |
|
46 |
insertIntersection(4, 3, DOUBLE_C, CLEFT); |
|
47 |
insertIntersection(5, 3, DOUBLE_C, CTOP); |
|
48 |
insertIntersection(6, 3, DOUBLE_C, CRIGHT); |
|
49 |
insertIntersection(7, 3, DOUBLE_C, CBOT); |
|
50 |
insertIntersection(8, 5, DOUBLE_T, TLEFT); |
|
51 |
insertIntersection(9, 5, DOUBLE_T, TMIDDLE); |
|
52 |
insertIntersection(10, 5, DOUBLE_T, TRIGHT); |
|
53 |
insertIntersection(11, 6, ON_RAMP, R_LEFT); |
|
54 |
insertIntersection(12, 6, ON_RAMP, R_RAMP); |
|
55 |
insertIntersection(13, 6, ON_RAMP, R_RIGHT); |
|
56 |
insertIntersection(14, 4, OFF_RAMP, R_RIGHT); |
|
57 |
insertIntersection(15, 4, OFF_RAMP, R_RAMP); |
|
58 |
insertIntersection(16, 4, OFF_RAMP, R_LEFT); |
|
59 |
return; |
|
50 | 60 |
} |
51 | 61 |
|
52 | 62 |
// Functions to get data from the Database: |
53 | 63 |
|
54 | 64 |
int getFeatureId(int key) |
55 | 65 |
{ |
56 |
if(key < NUM_FEATURES)
|
|
57 |
return (NodeData[key]>>12)&15;
|
|
58 |
else return -1;
|
|
66 |
if(key < NUM_FEATURES)
|
|
67 |
return (NodeData[key]>>12)&15;
|
|
68 |
else return -1;
|
|
59 | 69 |
} |
60 | 70 |
|
61 | 71 |
int getIntersectType(int key){ |
62 |
if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
|
63 |
return (NodeData[key]>>2)&7;
|
|
64 |
else return -1;
|
|
72 |
if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
|
73 |
return (NodeData[key]>>2)&7;
|
|
74 |
else return -1;
|
|
65 | 75 |
} |
66 | 76 |
|
67 | 77 |
int getIntersectPos(int key){ |
68 |
if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
|
69 |
return (NodeData[key])&3;
|
|
70 |
else return -1;
|
|
78 |
if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
|
79 |
return (NodeData[key])&3;
|
|
80 |
else return -1;
|
|
71 | 81 |
} |
72 | 82 |
|
73 | 83 |
int getRoadType(int key){ |
74 |
if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)
|
|
75 |
return (NodeData[key]>>2)&7;
|
|
76 |
else return -1;
|
|
84 |
if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)
|
|
85 |
return (NodeData[key]>>2)&7;
|
|
86 |
else return -1;
|
|
77 | 87 |
} |
78 | 88 |
|
79 | 89 |
int getRoadInfo(int key){ |
80 |
if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)
|
|
81 |
return (NodeData[key])&3;
|
|
82 |
else return -1;
|
|
90 |
if(key < NUM_FEATURES && getFeatureId(key) == ROAD_NODE)
|
|
91 |
return (NodeData[key])&3;
|
|
92 |
else return -1;
|
|
83 | 93 |
} |
84 | 94 |
|
85 | 95 |
int getIntersectNum(int key){ |
86 |
if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
|
87 |
return (NodeData[key]>>5)&127;
|
|
88 |
else return -1;
|
|
96 |
if(key < NUM_FEATURES && getFeatureId(key) == INTERSECTION_NODE)
|
|
97 |
return (NodeData[key]>>5)&127;
|
|
98 |
else return -1;
|
|
89 | 99 |
} |
Also available in: Unified diff