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