root / trunk / code / projects / traffic_navigation / validTurns.c @ 1973
History | View | Annotate | Download (3.55 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 | 1966 | alevkoy | unsigned int seed = 0xC0DE; |
| 26 | 1958 | pdeo | int randomNumGen(int max){ |
| 27 | 1966 | alevkoy | //int a = 0; // not currently in use
|
| 28 | //int b = 0xBEEF;
|
||
| 29 | 1958 | pdeo | seed++; |
| 30 | return seed%4; |
||
| 31 | } |
||
| 32 | 1866 | pdeo | |
| 33 | 1905 | pdeo | /*
|
| 34 | 1866 | pdeo | int randomNumGen(int max){
|
| 35 | 1913 | bwasserm | return rtc_get() % max;
|
| 36 | 1866 | pdeo | }
|
| 37 | 1905 | pdeo | int randomNumGen(int max){
|
| 38 | int a = 1337;
|
||
| 39 | 1912 | pdeo | int b = 0xDEADBEEF;
|
| 40 | 1913 | bwasserm | seed++;
|
| 41 | 1905 | pdeo | return seed%max;
|
| 42 | }
|
||
| 43 | 1966 | alevkoy | */
|
| 44 | 1905 | pdeo | |
| 45 | 1896 | bwasserm | /*
|
| 46 | 1886 | pdeo | int getIntersectType(int barcode)
|
| 47 | 1866 | pdeo | {
|
| 48 | 1912 | pdeo | return randomNumGen(5);
|
| 49 | 1866 | pdeo | }
|
| 50 | |||
| 51 | 1886 | pdeo | int getIntersectPos(int barcode, int max)
|
| 52 | 1866 | pdeo | {
|
| 53 | return randomNumGen(max);
|
||
| 54 | }
|
||
| 55 | 1896 | bwasserm | */
|
| 56 | 1966 | alevkoy | |
| 57 | 1879 | pdeo | int getTurnType(int barcode) |
| 58 | 1866 | pdeo | {
|
| 59 | return randomNumGen(4); |
||
| 60 | } |
||
| 61 | |||
| 62 | 1879 | pdeo | int validateTurn(int barcode, int turn_type) |
| 63 | 1866 | pdeo | {
|
| 64 | 1886 | pdeo | int intersect_type;
|
| 65 | int intersect_pos;
|
||
| 66 | intersect_type = getIntersectType(barcode); |
||
| 67 | switch (intersect_type)
|
||
| 68 | 1866 | pdeo | {
|
| 69 | 1912 | pdeo | case DOUBLE_C:
|
| 70 | 1866 | pdeo | {
|
| 71 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
| 72 | if (0<=intersect_pos && intersect_pos<=3) |
||
| 73 | 1879 | pdeo | return turn_type;
|
| 74 | 1866 | pdeo | break;
|
| 75 | } |
||
| 76 | 1912 | pdeo | case DOUBLE_T:
|
| 77 | 1866 | pdeo | {
|
| 78 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
| 79 | switch (intersect_pos)
|
||
| 80 | 1867 | jdcooper | {
|
| 81 | case TLEFT:
|
||
| 82 | {
|
||
| 83 | if (turn_type == ILEFT) turn_type = ISTRAIGHT;
|
||
| 84 | return turn_type;
|
||
| 85 | break;
|
||
| 86 | } |
||
| 87 | case TRIGHT:
|
||
| 88 | {
|
||
| 89 | if (turn_type == IRIGHT) turn_type = ISTRAIGHT;
|
||
| 90 | return turn_type;
|
||
| 91 | break;
|
||
| 92 | } |
||
| 93 | case TMIDDLE:
|
||
| 94 | {
|
||
| 95 | if (turn_type == ISTRAIGHT) turn_type = IUTURN;
|
||
| 96 | return turn_type;
|
||
| 97 | break;
|
||
| 98 | } |
||
| 99 | default:
|
||
| 100 | return -1; |
||
| 101 | } |
||
| 102 | break;
|
||
| 103 | 1866 | pdeo | } |
| 104 | 1912 | pdeo | case SINGLE:
|
| 105 | 1866 | pdeo | {
|
| 106 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
| 107 | switch (intersect_pos)
|
||
| 108 | 1867 | jdcooper | {
|
| 109 | case SACROSS:
|
||
| 110 | {
|
||
| 111 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
| 112 | return turn_type;
|
||
| 113 | break;
|
||
| 114 | } |
||
| 115 | case SUP:
|
||
| 116 | {
|
||
| 117 | if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
| 118 | return turn_type;
|
||
| 119 | break;
|
||
| 120 | } |
||
| 121 | default:
|
||
| 122 | return -1; |
||
| 123 | } |
||
| 124 | 1866 | pdeo | break;
|
| 125 | } |
||
| 126 | 1912 | pdeo | case ON_RAMP:
|
| 127 | 1866 | pdeo | {
|
| 128 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
| 129 | switch (intersect_pos)
|
||
| 130 | 1867 | jdcooper | {
|
| 131 | case R_LEFT:
|
||
| 132 | {
|
||
| 133 | if (turn_type == ILEFT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
| 134 | return turn_type;
|
||
| 135 | break;
|
||
| 136 | } |
||
| 137 | case R_RIGHT:
|
||
| 138 | {
|
||
| 139 | if (turn_type == IRIGHT || turn_type == IUTURN) turn_type = ISTRAIGHT;
|
||
| 140 | return turn_type;
|
||
| 141 | break;
|
||
| 142 | } |
||
| 143 | default:
|
||
| 144 | return -1; |
||
| 145 | } |
||
| 146 | 1866 | pdeo | break;
|
| 147 | } |
||
| 148 | 1912 | pdeo | case OFF_RAMP:
|
| 149 | 1866 | pdeo | {
|
| 150 | 1886 | pdeo | intersect_pos = getIntersectPos(barcode); |
| 151 | switch (intersect_pos)
|
||
| 152 | 1867 | jdcooper | {
|
| 153 | case R_LEFT:
|
||
| 154 | {
|
||
| 155 | int turn_type = ISTRAIGHT;
|
||
| 156 | return turn_type;
|
||
| 157 | break;
|
||
| 158 | } |
||
| 159 | case R_RIGHT:
|
||
| 160 | {
|
||
| 161 | int turn_type = ISTRAIGHT;
|
||
| 162 | return turn_type;
|
||
| 163 | break;
|
||
| 164 | } |
||
| 165 | case R_RAMP:
|
||
| 166 | {
|
||
| 167 | if (turn_type == ISTRAIGHT || turn_type == IUTURN) turn_type = ILEFT;
|
||
| 168 | return turn_type;
|
||
| 169 | break;
|
||
| 170 | } |
||
| 171 | default:
|
||
| 172 | return -1; |
||
| 173 | } |
||
| 174 | 1866 | pdeo | break;
|
| 175 | } |
||
| 176 | 1966 | alevkoy | default:
|
| 177 | return -1; |
||
| 178 | } |
||
| 179 | |||
| 180 | return -1; |
||
| 181 | 1866 | pdeo | } |