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