root / trunk / code / projects / traffic_navigation / validTurns.c @ 1884
History | View | Annotate | Download (3.28 KB)
1 | 1866 | pdeo | /*
|
---|---|---|---|
2 | 1881 | pdeo | * 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 | 1866 | pdeo | *
|
8 | */
|
||
9 | |||
10 | 1879 | pdeo | #include <dragonfly_lib.h> |
11 | 1866 | pdeo | #include "validTurns.h" |
12 | |||
13 | 1881 | pdeo | |
14 | /******************************Random Num Gen Version of validTurns
|
||
15 | 1866 | pdeo | 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 | 1879 | pdeo | int getTurnType(int barcode)
|
38 | 1866 | pdeo | {
|
39 | return randomNumGen(4);
|
||
40 | }
|
||
41 | 1879 | pdeo | */
|
42 | 1866 | pdeo | |
43 | 1879 | pdeo | int getCrossType(int barcode){ |
44 | 1882 | pdeo | int crosstype = (barcode>>2)&7; |
45 | 1879 | pdeo | return crosstype;
|
46 | } |
||
47 | int getCrossPos(int barcode, int max){ |
||
48 | 1884 | jdcooper | //@TODO: implement some max checking
|
49 | 1879 | pdeo | return (barcode)&3; |
50 | } |
||
51 | |||
52 | int validateTurn(int barcode, int turn_type) |
||
53 | 1866 | pdeo | { |
54 | int cross_type;
|
||
55 | int cross_pos;
|
||
56 | cross_type = getCrossType(barcode); |
||
57 | switch (cross_type)
|
||
58 | { |
||
59 | 1884 | jdcooper | case INTERSECTION_DOUBLE_C:
|
60 | 1866 | pdeo | { |
61 | 1867 | jdcooper | cross_pos = getCrossPos(barcode, 4);
|
62 | if (0<=cross_pos && cross_pos<=3) |
||
63 | 1879 | pdeo | return turn_type;
|
64 | 1866 | pdeo | break;
|
65 | } |
||
66 | 1881 | pdeo | case DOUBLE: //Implements DOUBLE as DOUBLE_T |
67 | 1884 | jdcooper | //@TODO: we really shouldn't use DOUBLE, it's part of another system. but it equals 0, so error checking.
|
68 | case INTERSECTION_DOUBLE_T:
|
||
69 | 1866 | pdeo | { |
70 | 1867 | jdcooper | cross_pos = getCrossPos(barcode, 3);
|
71 | switch (cross_pos)
|
||
72 | { |
||
73 | case TLEFT:
|
||
74 | { |
||
75 | if (turn_type == ILEFT) turn_type = ISTRAIGHT;
|
||
76 | return turn_type;
|
||
77 | break;
|
||
78 | } |
||
79 | case TRIGHT:
|
||
80 | { |
||
81 | if (turn_type == IRIGHT) turn_type = ISTRAIGHT;
|
||
82 | return turn_type;
|
||
83 | break;
|
||
84 | } |
||
85 | case TMIDDLE:
|
||
86 | { |
||
87 | if (turn_type == ISTRAIGHT) turn_type = IUTURN;
|
||
88 | return turn_type;
|
||
89 | break;
|
||
90 | } |
||
91 | default:
|
||
92 | return -1; |
||
93 | } |
||
94 | break;
|
||
95 | 1866 | pdeo | } |
96 | 1884 | jdcooper | case INTERSECTION_SINGLE:
|
97 | 1866 | pdeo | { |
98 | 1867 | jdcooper | cross_pos = getCrossPos(barcode, 2);
|
99 | switch (cross_pos)
|
||
100 | { |
||
101 | case SACROSS:
|
||
102 | { |
||
103 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
104 | return turn_type;
|
||
105 | break;
|
||
106 | } |
||
107 | case SUP:
|
||
108 | { |
||
109 | if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
110 | return turn_type;
|
||
111 | break;
|
||
112 | } |
||
113 | default:
|
||
114 | return -1; |
||
115 | } |
||
116 | 1866 | pdeo | break;
|
117 | } |
||
118 | 1884 | jdcooper | case INTERSECTION_ON_RAMP:
|
119 | 1866 | pdeo | { |
120 | 1867 | jdcooper | cross_pos = getCrossPos(barcode, 3);
|
121 | switch (cross_pos)
|
||
122 | { |
||
123 | case R_LEFT:
|
||
124 | { |
||
125 | if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
126 | return turn_type;
|
||
127 | break;
|
||
128 | } |
||
129 | case R_RIGHT:
|
||
130 | { |
||
131 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
132 | return turn_type;
|
||
133 | break;
|
||
134 | } |
||
135 | default:
|
||
136 | return -1; |
||
137 | } |
||
138 | 1866 | pdeo | break;
|
139 | } |
||
140 | 1884 | jdcooper | case INTERSECTION_OFF_RAMP:
|
141 | 1866 | pdeo | { |
142 | 1867 | jdcooper | cross_pos = getCrossPos(barcode, 3);
|
143 | switch (cross_pos)
|
||
144 | { |
||
145 | case R_LEFT:
|
||
146 | { |
||
147 | int turn_type = ISTRAIGHT;
|
||
148 | return turn_type;
|
||
149 | break;
|
||
150 | } |
||
151 | case R_RIGHT:
|
||
152 | { |
||
153 | int turn_type = ISTRAIGHT;
|
||
154 | return turn_type;
|
||
155 | break;
|
||
156 | } |
||
157 | case R_RAMP:
|
||
158 | { |
||
159 | if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT;
|
||
160 | return turn_type;
|
||
161 | break;
|
||
162 | } |
||
163 | default:
|
||
164 | return -1; |
||
165 | } |
||
166 | 1866 | pdeo | break;
|
167 | } |
||
168 | 1867 | jdcooper | default:
|
169 | return -1; |
||
170 | 1866 | pdeo | } |
171 | 1867 | jdcooper | } |