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