root / trunk / code / projects / traffic_navigation / validTurns.c @ 1881
History | View | Annotate | Download (3.07 KB)
1 |
/*
|
---|---|
2 |
* Deterministic turning implemented using table lookup. Using a
|
3 |
* table, the function looks up a value from the table using the
|
4 |
* barcode value, and then the robot decides to go straight,
|
5 |
* left, right, or u-turn depending on the valid turn types
|
6 |
* for the intersection it is at.
|
7 |
*
|
8 |
*/
|
9 |
|
10 |
#include <dragonfly_lib.h> |
11 |
#include "validTurns.h" |
12 |
|
13 |
|
14 |
/******************************Random Num Gen Version of validTurns
|
15 |
int randomNumGen(int max){
|
16 |
int x = range_read_distance(IR2);
|
17 |
if (x>0) return range_read_distance(IR2)%max;
|
18 |
else return randomNumGen(max);
|
19 |
}
|
20 |
|
21 |
int getCrossType(int barcode)
|
22 |
{
|
23 |
int x = randomNumGen(4);
|
24 |
if (x == DOUBLE) {
|
25 |
int y = randomNumGen(2);
|
26 |
if (y == 0) x = DOUBLE_C;
|
27 |
else x = DOUBLE_T;
|
28 |
}
|
29 |
return x;
|
30 |
}
|
31 |
|
32 |
int getCrossPos(int barcode, int max)
|
33 |
{
|
34 |
return randomNumGen(max);
|
35 |
}
|
36 |
|
37 |
int getTurnType(int barcode)
|
38 |
{
|
39 |
return randomNumGen(4);
|
40 |
}
|
41 |
*/
|
42 |
|
43 |
int getCrossType(int barcode){ |
44 |
int crosstype = (barcode>>2)&3; |
45 |
return crosstype;
|
46 |
} |
47 |
int getCrossPos(int barcode, int max){ |
48 |
return (barcode)&3; |
49 |
} |
50 |
|
51 |
int validateTurn(int barcode, int turn_type) |
52 |
{ |
53 |
int cross_type;
|
54 |
int cross_pos;
|
55 |
cross_type = getCrossType(barcode); |
56 |
switch (cross_type)
|
57 |
{ |
58 |
case DOUBLE_C:
|
59 |
{ |
60 |
cross_pos = getCrossPos(barcode, 4);
|
61 |
if (0<=cross_pos && cross_pos<=3) |
62 |
return turn_type;
|
63 |
break;
|
64 |
} |
65 |
case DOUBLE: //Implements DOUBLE as DOUBLE_T |
66 |
case DOUBLE_T:
|
67 |
{ |
68 |
cross_pos = getCrossPos(barcode, 3);
|
69 |
switch (cross_pos)
|
70 |
{ |
71 |
case TLEFT:
|
72 |
{ |
73 |
if (turn_type == ILEFT) turn_type = ISTRAIGHT;
|
74 |
return turn_type;
|
75 |
break;
|
76 |
} |
77 |
case TRIGHT:
|
78 |
{ |
79 |
if (turn_type == IRIGHT) turn_type = ISTRAIGHT;
|
80 |
return turn_type;
|
81 |
break;
|
82 |
} |
83 |
case TMIDDLE:
|
84 |
{ |
85 |
if (turn_type == ISTRAIGHT) turn_type = IUTURN;
|
86 |
return turn_type;
|
87 |
break;
|
88 |
} |
89 |
default:
|
90 |
return -1; |
91 |
} |
92 |
break;
|
93 |
} |
94 |
case SINGLE:
|
95 |
{ |
96 |
cross_pos = getCrossPos(barcode, 2);
|
97 |
switch (cross_pos)
|
98 |
{ |
99 |
case SACROSS:
|
100 |
{ |
101 |
if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
102 |
return turn_type;
|
103 |
break;
|
104 |
} |
105 |
case SUP:
|
106 |
{ |
107 |
if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
108 |
return turn_type;
|
109 |
break;
|
110 |
} |
111 |
default:
|
112 |
return -1; |
113 |
} |
114 |
break;
|
115 |
} |
116 |
case ON_RAMP:
|
117 |
{ |
118 |
cross_pos = getCrossPos(barcode, 3);
|
119 |
switch (cross_pos)
|
120 |
{ |
121 |
case R_LEFT:
|
122 |
{ |
123 |
if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
124 |
return turn_type;
|
125 |
break;
|
126 |
} |
127 |
case R_RIGHT:
|
128 |
{ |
129 |
if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
130 |
return turn_type;
|
131 |
break;
|
132 |
} |
133 |
default:
|
134 |
return -1; |
135 |
} |
136 |
break;
|
137 |
} |
138 |
case OFF_RAMP:
|
139 |
{ |
140 |
cross_pos = getCrossPos(barcode, 3);
|
141 |
switch (cross_pos)
|
142 |
{ |
143 |
case R_LEFT:
|
144 |
{ |
145 |
int turn_type = ISTRAIGHT;
|
146 |
return turn_type;
|
147 |
break;
|
148 |
} |
149 |
case R_RIGHT:
|
150 |
{ |
151 |
int turn_type = ISTRAIGHT;
|
152 |
return turn_type;
|
153 |
break;
|
154 |
} |
155 |
case R_RAMP:
|
156 |
{ |
157 |
if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT;
|
158 |
return turn_type;
|
159 |
break;
|
160 |
} |
161 |
default:
|
162 |
return -1; |
163 |
} |
164 |
break;
|
165 |
} |
166 |
default:
|
167 |
return -1; |
168 |
} |
169 |
} |