root / trunk / code / projects / traffic_navigation / validTurns.c @ 1866
History | View | Annotate | Download (2.7 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 |
case DOUBLE_C
|
46 |
{ |
47 |
cross_pos = getCrossPos(barcode, 4);
|
48 |
if (0<=cross_pos && cross_pos<=3) |
49 |
return randomNumGen(4); |
50 |
break;
|
51 |
} |
52 |
case DOUBLE_T
|
53 |
{ |
54 |
cross_pos = getCrossPos(barcode, 3);
|
55 |
switch (cross_pos)
|
56 |
case TLEFT
|
57 |
{ |
58 |
int turn_type = getTurnType();
|
59 |
if (turn_type == ILEFT) turn_type = ISTRAIGHT;
|
60 |
return turn_type;
|
61 |
break;
|
62 |
} |
63 |
case TRIGHT
|
64 |
{ |
65 |
int turn_type = getTurnType();
|
66 |
if (turn_type == IRIGHT) turn_type = ISTRAIGHT;
|
67 |
return turn_type;
|
68 |
break;
|
69 |
} |
70 |
case TMIDDLE
|
71 |
{ |
72 |
int turn_type = getTurnType();
|
73 |
if (turn_type == ISTRAIGHT) turn_type = IUTURN;
|
74 |
return turn_type;
|
75 |
break;
|
76 |
} |
77 |
default return -1; |
78 |
break;
|
79 |
} |
80 |
case SINGLE
|
81 |
{ |
82 |
cross_pos = getCrossPos(barcode, 2);
|
83 |
switch (cross_pos)
|
84 |
case SACROSS
|
85 |
{ |
86 |
int turn_type = getTurnType();
|
87 |
if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
88 |
return turn_type;
|
89 |
break;
|
90 |
} |
91 |
case SUP
|
92 |
{ |
93 |
int turn_type = getTurnType();
|
94 |
if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
95 |
return turn_type;
|
96 |
break;
|
97 |
} |
98 |
default return -1; |
99 |
break;
|
100 |
} |
101 |
case ON_RAMP
|
102 |
{ |
103 |
cross_pos = getCrossPos(barcode, 3);
|
104 |
switch (cross_pos)
|
105 |
case R_LEFT
|
106 |
{ |
107 |
int turn_type = getTurnType();
|
108 |
if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
109 |
return turn_type;
|
110 |
break;
|
111 |
} |
112 |
case R_RIGHT
|
113 |
{ |
114 |
int turn_type = getTurnType();
|
115 |
if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
116 |
return turn_type;
|
117 |
break;
|
118 |
} |
119 |
default return -1; |
120 |
break;
|
121 |
} |
122 |
case OFF_RAMP
|
123 |
{ |
124 |
cross_pos = getCrossPos(barcode, 3);
|
125 |
switch (cross_pos)
|
126 |
case R_LEFT
|
127 |
{ |
128 |
int turn_type = ISTRAIGHT;
|
129 |
return turn_type;
|
130 |
break;
|
131 |
} |
132 |
case R_RIGHT
|
133 |
{ |
134 |
int turn_type = ISTRAIGHT;
|
135 |
return turn_type;
|
136 |
break;
|
137 |
} |
138 |
case R_RAMP
|
139 |
{ |
140 |
int turn_type = getTurnType();
|
141 |
if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT;
|
142 |
return turn_type;
|
143 |
break;
|
144 |
} |
145 |
default return -1; |
146 |
break;
|
147 |
} |
148 |
default return -1; |
149 |
} |