Revision 1879
tested validTurns.c and made it more compatible with the upcoming database.
trunk/code/projects/traffic_navigation/validTurns.h | ||
---|---|---|
1 | 1 |
#ifndef _VALID_TURNS_ |
2 | 2 |
#define _VALID_TURNS_ |
3 | 3 |
|
4 |
#include "../linefollowing/lineFollow.h" |
|
5 |
#include "../linefollowing/lineDrive.h" |
|
6 | 4 |
|
5 |
#include "lineDrive.h" |
|
6 |
|
|
7 | 7 |
//To differenciate between T and Cross Intersections |
8 | 8 |
#define DOUBLE_C 4 |
9 | 9 |
#define DOUBLE_T 5 |
... | ... | |
54 | 54 |
|
55 | 55 |
int getCrossPos(int barcode, int max); |
56 | 56 |
|
57 |
int getTurnType();
|
|
57 |
int validateTurn(int barcode, int turn_type);
|
|
58 | 58 |
|
59 |
int validateTurn(int barcode); |
|
60 |
|
|
61 | 59 |
#endif |
trunk/code/projects/traffic_navigation/validTurns.c | ||
---|---|---|
5 | 5 |
* |
6 | 6 |
*/ |
7 | 7 |
|
8 |
|
|
8 |
#include <dragonfly_lib.h> |
|
9 | 9 |
#include "validTurns.h" |
10 | 10 |
|
11 |
#define NOBARCODE -1 |
|
12 |
/* |
|
11 | 13 |
int randomNumGen(int max){ |
12 | 14 |
int x = range_read_distance(IR2); |
13 | 15 |
if (x>0) return range_read_distance(IR2)%max; |
... | ... | |
30 | 32 |
return randomNumGen(max); |
31 | 33 |
} |
32 | 34 |
|
33 |
int getTurnType() |
|
35 |
int getTurnType(int barcode)
|
|
34 | 36 |
{ |
35 | 37 |
return randomNumGen(4); |
36 | 38 |
} |
39 |
*/ |
|
37 | 40 |
|
38 |
int validateTurn(int barcode) |
|
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) |
|
39 | 51 |
{ |
40 | 52 |
int cross_type; |
41 | 53 |
int cross_pos; |
... | ... | |
47 | 59 |
{ |
48 | 60 |
cross_pos = getCrossPos(barcode, 4); |
49 | 61 |
if (0<=cross_pos && cross_pos<=3) |
50 |
return randomNumGen(4);
|
|
62 |
return turn_type;
|
|
51 | 63 |
break; |
52 | 64 |
} |
53 | 65 |
case DOUBLE_T: |
... | ... | |
57 | 69 |
{ |
58 | 70 |
case TLEFT: |
59 | 71 |
{ |
60 |
int turn_type = getTurnType(); |
|
61 | 72 |
if (turn_type == ILEFT) turn_type = ISTRAIGHT; |
62 | 73 |
return turn_type; |
63 | 74 |
break; |
64 | 75 |
} |
65 | 76 |
case TRIGHT: |
66 | 77 |
{ |
67 |
int turn_type = getTurnType(); |
|
68 | 78 |
if (turn_type == IRIGHT) turn_type = ISTRAIGHT; |
69 | 79 |
return turn_type; |
70 | 80 |
break; |
71 | 81 |
} |
72 | 82 |
case TMIDDLE: |
73 | 83 |
{ |
74 |
int turn_type = getTurnType(); |
|
75 | 84 |
if (turn_type == ISTRAIGHT) turn_type = IUTURN; |
76 | 85 |
return turn_type; |
77 | 86 |
break; |
... | ... | |
88 | 97 |
{ |
89 | 98 |
case SACROSS: |
90 | 99 |
{ |
91 |
int turn_type = getTurnType(); |
|
92 | 100 |
if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT; |
93 | 101 |
return turn_type; |
94 | 102 |
break; |
95 | 103 |
} |
96 | 104 |
case SUP: |
97 | 105 |
{ |
98 |
int turn_type = getTurnType(); |
|
99 | 106 |
if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT; |
100 | 107 |
return turn_type; |
101 | 108 |
break; |
... | ... | |
112 | 119 |
{ |
113 | 120 |
case R_LEFT: |
114 | 121 |
{ |
115 |
int turn_type = getTurnType(); |
|
116 | 122 |
if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT; |
117 | 123 |
return turn_type; |
118 | 124 |
break; |
119 | 125 |
} |
120 | 126 |
case R_RIGHT: |
121 | 127 |
{ |
122 |
int turn_type = getTurnType(); |
|
123 | 128 |
if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT; |
124 | 129 |
return turn_type; |
125 | 130 |
break; |
... | ... | |
148 | 153 |
} |
149 | 154 |
case R_RAMP: |
150 | 155 |
{ |
151 |
int turn_type = getTurnType(); |
|
152 | 156 |
if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT; |
153 | 157 |
return turn_type; |
154 | 158 |
break; |
Also available in: Unified diff