root / trunk / code / projects / traffic_navigation / validTurns.c @ 2008
History | View | Annotate | Download (3.69 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 | 1958 | pdeo | |
11 | /******************************Random Num Gen Version of validTurns*/
|
||
12 | |||
13 | 1966 | alevkoy | #include "intersectData.h" |
14 | #include "../linefollowing/lineDrive.h" |
||
15 | #include "validTurns.h" |
||
16 | 1958 | pdeo | |
17 | #define GET16(s)( ((s)&0x8000)>>16 ) /*gets the 16 bit*/ |
||
18 | #define GET14(s)( ((s)&0x2000)>>14 ) /*gets the 14thbit*/ |
||
19 | #define GET13(s)( ((s)&0x1000)>>13 ) /*you get the idea*/ |
||
20 | #define GET11(s)( ((s)&0x400)>>11 ) |
||
21 | #define CYCLES 10 /*the number of reseeds i perform before finally extracting my ranodm number*/ |
||
22 | #define SHIFT(s)( (s)<<1) |
||
23 | #define RESEED(s)( (s)=( ( ((GET16(s)^GET14(s))^GET13(s))^GET11(s) ) | SHIFT(s) ) )/*reseeding the first bit of the number with bits from the number*/ |
||
24 | |||
25 | 1991 | azl | unsigned int turnseed = 0xC0DE; |
26 | 1993 | azl | int randomNumGen(int max){ |
27 | 1991 | azl | turnseed *= (rtc_get() + encoder_read(LEFT))%9;
|
28 | return turnseed % max;
|
||
29 | 1958 | pdeo | } |
30 | 1866 | pdeo | |
31 | 1905 | pdeo | /*
|
32 | 1886 | pdeo | int getIntersectType(int barcode)
|
33 | 1866 | pdeo | {
|
34 | 1912 | pdeo | return randomNumGen(5);
|
35 | 1866 | pdeo | }
|
36 | |||
37 | 1886 | pdeo | int getIntersectPos(int barcode, int max)
|
38 | 1866 | pdeo | {
|
39 | return randomNumGen(max);
|
||
40 | }
|
||
41 | 1896 | bwasserm | */
|
42 | 1966 | alevkoy | |
43 | 1993 | azl | int getTurnType(int barcode) |
44 | 1866 | pdeo | { |
45 | return randomNumGen(4); |
||
46 | } |
||
47 | |||
48 | 1879 | pdeo | int validateTurn(int barcode, int turn_type) |
49 | 1866 | pdeo | { |
50 | 1886 | pdeo | int intersect_type;
|
51 | int intersect_pos;
|
||
52 | intersect_type = getIntersectType(barcode); |
||
53 | 1988 | bwasserm | //TEMPORARY CODE FOR TEST MAP
|
54 | //INTERSECTION 2 (barcodes 13 and 16) was made incorrectly
|
||
55 | //If the bot reads barcode 16, it must turn left
|
||
56 | //If the bot reads barcode 13, it must go straight
|
||
57 | if(barcode == 16){ |
||
58 | return ILEFT;
|
||
59 | } |
||
60 | if(barcode == 13){ |
||
61 | return ISTRAIGHT;
|
||
62 | } |
||
63 | //END TEMP CODE
|
||
64 | |||
65 | switch (intersect_type)
|
||
66 | 1866 | pdeo | { |
67 | 1912 | pdeo | case DOUBLE_C:
|
68 | 1866 | pdeo | { |
69 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
70 | if (0<=intersect_pos && intersect_pos<=3) |
||
71 | 1879 | pdeo | return turn_type;
|
72 | 1866 | pdeo | break;
|
73 | } |
||
74 | 1912 | pdeo | case DOUBLE_T:
|
75 | 1866 | pdeo | { |
76 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
77 | switch (intersect_pos)
|
||
78 | 1867 | jdcooper | { |
79 | case TLEFT:
|
||
80 | { |
||
81 | if (turn_type == ILEFT) turn_type = ISTRAIGHT;
|
||
82 | return turn_type;
|
||
83 | break;
|
||
84 | } |
||
85 | case TRIGHT:
|
||
86 | { |
||
87 | if (turn_type == IRIGHT) turn_type = ISTRAIGHT;
|
||
88 | return turn_type;
|
||
89 | break;
|
||
90 | } |
||
91 | case TMIDDLE:
|
||
92 | { |
||
93 | if (turn_type == ISTRAIGHT) turn_type = IUTURN;
|
||
94 | return turn_type;
|
||
95 | break;
|
||
96 | } |
||
97 | default:
|
||
98 | return -1; |
||
99 | } |
||
100 | break;
|
||
101 | 1866 | pdeo | } |
102 | 1912 | pdeo | case SINGLE:
|
103 | 1866 | pdeo | { |
104 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
105 | switch (intersect_pos)
|
||
106 | 1867 | jdcooper | { |
107 | case SACROSS:
|
||
108 | { |
||
109 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
110 | return turn_type;
|
||
111 | break;
|
||
112 | } |
||
113 | case SUP:
|
||
114 | { |
||
115 | if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
116 | return turn_type;
|
||
117 | break;
|
||
118 | } |
||
119 | default:
|
||
120 | return -1; |
||
121 | } |
||
122 | 1866 | pdeo | break;
|
123 | } |
||
124 | 1912 | pdeo | case ON_RAMP:
|
125 | 1866 | pdeo | { |
126 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
127 | switch (intersect_pos)
|
||
128 | 1867 | jdcooper | { |
129 | case R_LEFT:
|
||
130 | { |
||
131 | if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
132 | return turn_type;
|
||
133 | break;
|
||
134 | } |
||
135 | case R_RIGHT:
|
||
136 | { |
||
137 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
138 | return turn_type;
|
||
139 | break;
|
||
140 | } |
||
141 | default:
|
||
142 | return -1; |
||
143 | } |
||
144 | 1866 | pdeo | break;
|
145 | } |
||
146 | 1912 | pdeo | case OFF_RAMP:
|
147 | 1866 | pdeo | { |
148 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
149 | switch (intersect_pos)
|
||
150 | 1867 | jdcooper | { |
151 | case R_LEFT:
|
||
152 | { |
||
153 | int turn_type = ISTRAIGHT;
|
||
154 | return turn_type;
|
||
155 | break;
|
||
156 | } |
||
157 | case R_RIGHT:
|
||
158 | { |
||
159 | int turn_type = ISTRAIGHT;
|
||
160 | return turn_type;
|
||
161 | break;
|
||
162 | } |
||
163 | case R_RAMP:
|
||
164 | { |
||
165 | if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT;
|
||
166 | return turn_type;
|
||
167 | break;
|
||
168 | } |
||
169 | default:
|
||
170 | return -1; |
||
171 | } |
||
172 | 1866 | pdeo | break;
|
173 | } |
||
174 | 1966 | alevkoy | default:
|
175 | return -1; |
||
176 | } |
||
177 | |||
178 | return -1; |
||
179 | 1866 | pdeo | } |