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