root / trunk / code / projects / traffic_navigation / validTurns.c @ 1901
History | View | Annotate | Download (3.03 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 | 1896 | bwasserm | #include <dragonfly_lib.h> |
11 | 1866 | pdeo | |
12 | 1896 | bwasserm | /******************************Random Num Gen Version of validTurns*/
|
13 | 1866 | pdeo | int randomNumGen(int max){ |
14 | 1901 | bwasserm | int x = rtc_get();
|
15 | 1866 | pdeo | if (x>0) return range_read_distance(IR2)%max; |
16 | else return randomNumGen(max); |
||
17 | } |
||
18 | 1896 | bwasserm | /*
|
19 | 1886 | pdeo | int getIntersectType(int barcode)
|
20 | 1866 | pdeo | {
|
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 | 1886 | pdeo | int getIntersectPos(int barcode, int max)
|
31 | 1866 | pdeo | {
|
32 | return randomNumGen(max);
|
||
33 | }
|
||
34 | 1896 | bwasserm | */
|
35 | 1879 | pdeo | int getTurnType(int barcode) |
36 | 1866 | pdeo | { |
37 | return randomNumGen(4); |
||
38 | } |
||
39 | |||
40 | 1885 | pdeo | |
41 | 1886 | pdeo | #include "intersectData.h" |
42 | #include "validTurns.h" |
||
43 | |||
44 | 1879 | pdeo | int validateTurn(int barcode, int turn_type) |
45 | 1866 | pdeo | { |
46 | 1886 | pdeo | int intersect_type;
|
47 | int intersect_pos;
|
||
48 | intersect_type = getIntersectType(barcode); |
||
49 | switch (intersect_type)
|
||
50 | 1866 | pdeo | { |
51 | 1884 | jdcooper | case INTERSECTION_DOUBLE_C:
|
52 | 1866 | pdeo | { |
53 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
54 | if (0<=intersect_pos && intersect_pos<=3) |
||
55 | 1879 | pdeo | return turn_type;
|
56 | 1866 | pdeo | break;
|
57 | } |
||
58 | 1884 | jdcooper | case INTERSECTION_DOUBLE_T:
|
59 | 1866 | pdeo | { |
60 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
61 | switch (intersect_pos)
|
||
62 | 1867 | jdcooper | { |
63 | case TLEFT:
|
||
64 | { |
||
65 | if (turn_type == ILEFT) turn_type = ISTRAIGHT;
|
||
66 | return turn_type;
|
||
67 | break;
|
||
68 | } |
||
69 | case TRIGHT:
|
||
70 | { |
||
71 | if (turn_type == IRIGHT) turn_type = ISTRAIGHT;
|
||
72 | return turn_type;
|
||
73 | break;
|
||
74 | } |
||
75 | case TMIDDLE:
|
||
76 | { |
||
77 | if (turn_type == ISTRAIGHT) turn_type = IUTURN;
|
||
78 | return turn_type;
|
||
79 | break;
|
||
80 | } |
||
81 | default:
|
||
82 | return -1; |
||
83 | } |
||
84 | break;
|
||
85 | 1866 | pdeo | } |
86 | 1884 | jdcooper | case INTERSECTION_SINGLE:
|
87 | 1866 | pdeo | { |
88 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
89 | switch (intersect_pos)
|
||
90 | 1867 | jdcooper | { |
91 | case SACROSS:
|
||
92 | { |
||
93 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
94 | return turn_type;
|
||
95 | break;
|
||
96 | } |
||
97 | case SUP:
|
||
98 | { |
||
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 | 1866 | pdeo | break;
|
107 | } |
||
108 | 1884 | jdcooper | case INTERSECTION_ON_RAMP:
|
109 | 1866 | pdeo | { |
110 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
111 | switch (intersect_pos)
|
||
112 | 1867 | jdcooper | { |
113 | case R_LEFT:
|
||
114 | { |
||
115 | if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
116 | return turn_type;
|
||
117 | break;
|
||
118 | } |
||
119 | case R_RIGHT:
|
||
120 | { |
||
121 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
122 | return turn_type;
|
||
123 | break;
|
||
124 | } |
||
125 | default:
|
||
126 | return -1; |
||
127 | } |
||
128 | 1866 | pdeo | break;
|
129 | } |
||
130 | 1884 | jdcooper | case INTERSECTION_OFF_RAMP:
|
131 | 1866 | pdeo | { |
132 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
133 | switch (intersect_pos)
|
||
134 | 1867 | jdcooper | { |
135 | case R_LEFT:
|
||
136 | { |
||
137 | int turn_type = ISTRAIGHT;
|
||
138 | return turn_type;
|
||
139 | break;
|
||
140 | } |
||
141 | case R_RIGHT:
|
||
142 | { |
||
143 | int turn_type = ISTRAIGHT;
|
||
144 | return turn_type;
|
||
145 | break;
|
||
146 | } |
||
147 | case R_RAMP:
|
||
148 | { |
||
149 | if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT;
|
||
150 | return turn_type;
|
||
151 | break;
|
||
152 | } |
||
153 | default:
|
||
154 | return -1; |
||
155 | } |
||
156 | 1866 | pdeo | break;
|
157 | } |
||
158 | 1867 | jdcooper | default:
|
159 | return -1; |
||
160 | 1866 | pdeo | } |
161 | 1867 | jdcooper | } |