![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Shape algorithm for location mapping.
Fork of Orientation_PerTile by
main.cpp@2:8667325e74a9, 2018-05-25 (annotated)
- Committer:
- el15tcd
- Date:
- Fri May 25 23:24:23 2018 +0000
- Revision:
- 2:8667325e74a9
- Parent:
- 1:c573caf40864
- Child:
- 3:87945bd950e8
Orientation algorithm to be applied per tile.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
el15tcd | 0:8a61e3541a5e | 1 | #include "mbed.h" |
el15tcd | 0:8a61e3541a5e | 2 | |
el15tcd | 0:8a61e3541a5e | 3 | BusOut myleds(LED4, LED3, LED2, LED1); |
el15tcd | 2:8667325e74a9 | 4 | DigitalOut NorthLED(LED1); //update pin when known |
el15tcd | 2:8667325e74a9 | 5 | DigitalOut EastLED(LED2); //update pin when known |
el15tcd | 2:8667325e74a9 | 6 | DigitalOut SouthLED(LED3); //update pin when known |
el15tcd | 2:8667325e74a9 | 7 | DigitalOut WestLED(LED4); //update pin when known |
el15tcd | 0:8a61e3541a5e | 8 | |
el15tcd | 2:8667325e74a9 | 9 | DigitalIn NorthDetect(p5); |
el15tcd | 2:8667325e74a9 | 10 | DigitalIn EastDetect(p6); |
el15tcd | 2:8667325e74a9 | 11 | DigitalIn SouthDetect(p7); |
el15tcd | 2:8667325e74a9 | 12 | DigitalIn WestDetect(p8); |
el15tcd | 2:8667325e74a9 | 13 | |
el15tcd | 2:8667325e74a9 | 14 | int NorthPattern[8]; |
el15tcd | 2:8667325e74a9 | 15 | int EastPattern[8]; |
el15tcd | 2:8667325e74a9 | 16 | int SouthPattern[8]; |
el15tcd | 2:8667325e74a9 | 17 | int WestPattern[8]; |
el15tcd | 2:8667325e74a9 | 18 | |
el15tcd | 2:8667325e74a9 | 19 | int NorthPattern2[8]; |
el15tcd | 2:8667325e74a9 | 20 | int EastPattern2[8]; |
el15tcd | 2:8667325e74a9 | 21 | int SouthPattern2[8]; |
el15tcd | 2:8667325e74a9 | 22 | int WestPattern2[8]; |
el15tcd | 2:8667325e74a9 | 23 | |
el15tcd | 2:8667325e74a9 | 24 | int NorthPattern3[8]; |
el15tcd | 2:8667325e74a9 | 25 | int EastPattern3[8]; |
el15tcd | 2:8667325e74a9 | 26 | int SouthPattern3[8]; |
el15tcd | 2:8667325e74a9 | 27 | int WestPattern3[8]; |
el15tcd | 0:8a61e3541a5e | 28 | |
el15tcd | 2:8667325e74a9 | 29 | int NorthPattern4[8]; |
el15tcd | 2:8667325e74a9 | 30 | int EastPattern4[8]; |
el15tcd | 2:8667325e74a9 | 31 | int SouthPattern4[8]; |
el15tcd | 2:8667325e74a9 | 32 | int WestPattern4[8]; |
el15tcd | 2:8667325e74a9 | 33 | |
el15tcd | 2:8667325e74a9 | 34 | int NorthDetected[5]; |
el15tcd | 2:8667325e74a9 | 35 | int EastDetected[5]; |
el15tcd | 2:8667325e74a9 | 36 | int SouthDetected[5]; |
el15tcd | 2:8667325e74a9 | 37 | int WestDetected[5]; |
el15tcd | 2:8667325e74a9 | 38 | |
el15tcd | 2:8667325e74a9 | 39 | //counters |
el15tcd | 2:8667325e74a9 | 40 | int N1N2; |
el15tcd | 2:8667325e74a9 | 41 | int N1N3; |
el15tcd | 2:8667325e74a9 | 42 | int N1N4; |
el15tcd | 2:8667325e74a9 | 43 | int N1E2; |
el15tcd | 2:8667325e74a9 | 44 | int N1E3; |
el15tcd | 2:8667325e74a9 | 45 | int N1E4; |
el15tcd | 2:8667325e74a9 | 46 | int N1S2; |
el15tcd | 2:8667325e74a9 | 47 | int N1S3; |
el15tcd | 2:8667325e74a9 | 48 | int N1S4; |
el15tcd | 2:8667325e74a9 | 49 | int N1W2; |
el15tcd | 2:8667325e74a9 | 50 | int N1W3; |
el15tcd | 2:8667325e74a9 | 51 | int N1W4; |
el15tcd | 0:8a61e3541a5e | 52 | |
el15tcd | 2:8667325e74a9 | 53 | int E1N2; |
el15tcd | 2:8667325e74a9 | 54 | int E1N3; |
el15tcd | 2:8667325e74a9 | 55 | int E1N4; |
el15tcd | 2:8667325e74a9 | 56 | int E1E2; |
el15tcd | 2:8667325e74a9 | 57 | int E1E3; |
el15tcd | 2:8667325e74a9 | 58 | int E1E4; |
el15tcd | 2:8667325e74a9 | 59 | int E1S2; |
el15tcd | 2:8667325e74a9 | 60 | int E1S3; |
el15tcd | 2:8667325e74a9 | 61 | int E1S4; |
el15tcd | 2:8667325e74a9 | 62 | int E1W2; |
el15tcd | 2:8667325e74a9 | 63 | int E1W3; |
el15tcd | 2:8667325e74a9 | 64 | int E1W4; |
el15tcd | 2:8667325e74a9 | 65 | |
el15tcd | 2:8667325e74a9 | 66 | int S1N2; |
el15tcd | 2:8667325e74a9 | 67 | int S1N3; |
el15tcd | 2:8667325e74a9 | 68 | int S1N4; |
el15tcd | 2:8667325e74a9 | 69 | int S1E2; |
el15tcd | 2:8667325e74a9 | 70 | int S1E3; |
el15tcd | 2:8667325e74a9 | 71 | int S1E4; |
el15tcd | 2:8667325e74a9 | 72 | int S1S2; |
el15tcd | 2:8667325e74a9 | 73 | int S1S3; |
el15tcd | 2:8667325e74a9 | 74 | int S1S4; |
el15tcd | 2:8667325e74a9 | 75 | int S1W2; |
el15tcd | 2:8667325e74a9 | 76 | int S1W3; |
el15tcd | 2:8667325e74a9 | 77 | int S1W4; |
el15tcd | 2:8667325e74a9 | 78 | |
el15tcd | 2:8667325e74a9 | 79 | int W1N2; |
el15tcd | 2:8667325e74a9 | 80 | int W1N3; |
el15tcd | 2:8667325e74a9 | 81 | int W1N4; |
el15tcd | 2:8667325e74a9 | 82 | int W1E2; |
el15tcd | 2:8667325e74a9 | 83 | int W1E3; |
el15tcd | 2:8667325e74a9 | 84 | int W1E4; |
el15tcd | 2:8667325e74a9 | 85 | int W1S2; |
el15tcd | 2:8667325e74a9 | 86 | int W1S3; |
el15tcd | 2:8667325e74a9 | 87 | int W1S4; |
el15tcd | 2:8667325e74a9 | 88 | int W1W2; |
el15tcd | 2:8667325e74a9 | 89 | int W1W3; |
el15tcd | 2:8667325e74a9 | 90 | int W1W4; |
el15tcd | 2:8667325e74a9 | 91 | |
el15tcd | 2:8667325e74a9 | 92 | int algorithm_update; |
el15tcd | 2:8667325e74a9 | 93 | int number_of_tiles; //value used to represent number of tiles |
el15tcd | 2:8667325e74a9 | 94 | |
el15tcd | 2:8667325e74a9 | 95 | //orientations : N=1, E=2, S=3, W=4 and Unknown = 0 |
el15tcd | 2:8667325e74a9 | 96 | int tile1_orientation; |
el15tcd | 2:8667325e74a9 | 97 | int tile2_orientation; |
el15tcd | 2:8667325e74a9 | 98 | int tile3_orientation; |
el15tcd | 2:8667325e74a9 | 99 | int tile4_orientation; |
el15tcd | 2:8667325e74a9 | 100 | |
el15tcd | 0:8a61e3541a5e | 101 | int i; |
el15tcd | 2:8667325e74a9 | 102 | int c; //detectors function counter |
el15tcd | 2:8667325e74a9 | 103 | int o; //detectors function variable |
el15tcd | 2:8667325e74a9 | 104 | int d; //detectors function variable |
el15tcd | 0:8a61e3541a5e | 105 | |
el15tcd | 2:8667325e74a9 | 106 | void patterns(int m); |
el15tcd | 2:8667325e74a9 | 107 | void patternsReset(); |
el15tcd | 2:8667325e74a9 | 108 | int detectors(int n); |
el15tcd | 0:8a61e3541a5e | 109 | |
el15tcd | 0:8a61e3541a5e | 110 | int main() |
el15tcd | 0:8a61e3541a5e | 111 | { |
el15tcd | 2:8667325e74a9 | 112 | //please note, some values would have to be adjusted to account for the extra 0 preceding the start bits |
el15tcd | 2:8667325e74a9 | 113 | /* |
el15tcd | 2:8667325e74a9 | 114 | initialise light patterns: |
el15tcd | 2:8667325e74a9 | 115 | start bit || data of defined length (which is 5 bits for 16 sensors) |
el15tcd | 2:8667325e74a9 | 116 | N1 = 111 00001 |
el15tcd | 2:8667325e74a9 | 117 | E1 = 111 00010 |
el15tcd | 2:8667325e74a9 | 118 | S1 = 111 00011 |
el15tcd | 2:8667325e74a9 | 119 | W1 = 111 00100 |
el15tcd | 2:8667325e74a9 | 120 | N2 = 111 00101 |
el15tcd | 2:8667325e74a9 | 121 | E2 = 111 00110 |
el15tcd | 2:8667325e74a9 | 122 | S2 = 111 01000 |
el15tcd | 2:8667325e74a9 | 123 | W2 = 111 01001 |
el15tcd | 2:8667325e74a9 | 124 | N3 = 111 01010 |
el15tcd | 2:8667325e74a9 | 125 | E3 = 111 01011 |
el15tcd | 2:8667325e74a9 | 126 | S3 = 111 01100 |
el15tcd | 2:8667325e74a9 | 127 | W3 = 111 01101 |
el15tcd | 2:8667325e74a9 | 128 | N4 = 111 10000 |
el15tcd | 2:8667325e74a9 | 129 | E4 = 111 10001 |
el15tcd | 2:8667325e74a9 | 130 | S4 = 111 10010 |
el15tcd | 2:8667325e74a9 | 131 | W4 = 111 10011 |
el15tcd | 0:8a61e3541a5e | 132 | |
el15tcd | 2:8667325e74a9 | 133 | 111 00111 |
el15tcd | 2:8667325e74a9 | 134 | 111 01110 These combinations skipped due to having 3 1's that are not the start bits |
el15tcd | 2:8667325e74a9 | 135 | 111 01111 |
el15tcd | 0:8a61e3541a5e | 136 | |
el15tcd | 2:8667325e74a9 | 137 | example of init below, different patterns for different tiles |
el15tcd | 2:8667325e74a9 | 138 | */ |
el15tcd | 2:8667325e74a9 | 139 | |
el15tcd | 2:8667325e74a9 | 140 | //tile 1 patterns (does not currently include the 0 preceding the start bits, as have had no sensors to test it further) |
el15tcd | 2:8667325e74a9 | 141 | NorthPattern[0] = 1; NorthPattern[1] = 1; NorthPattern[2] = 1; NorthPattern[3] = 0; NorthPattern[4] = 0; NorthPattern[5] = 0; NorthPattern[6] = 0; NorthPattern[7] = 1; |
el15tcd | 2:8667325e74a9 | 142 | EastPattern[0] = 1; EastPattern[1] = 1; EastPattern[2] = 1; EastPattern[3] = 0; EastPattern[4] = 0; EastPattern[5] = 0; EastPattern[6] = 1; EastPattern[7] = 0; |
el15tcd | 2:8667325e74a9 | 143 | SouthPattern[0] = 1; SouthPattern[1] = 1; SouthPattern[2] = 1; SouthPattern[3] = 0; SouthPattern[4] = 0; SouthPattern[5] = 0; SouthPattern[6] = 1; SouthPattern[7] = 1; |
el15tcd | 2:8667325e74a9 | 144 | WestPattern[0] = 1; WestPattern[1] = 1; WestPattern[2] = 1; WestPattern[3] = 0; WestPattern[4] = 0; WestPattern[5] = 1; WestPattern[6] = 0; WestPattern[7] = 0; |
el15tcd | 2:8667325e74a9 | 145 | |
el15tcd | 2:8667325e74a9 | 146 | //tile 2 patterns |
el15tcd | 2:8667325e74a9 | 147 | |
el15tcd | 2:8667325e74a9 | 148 | //tile 3 patterns |
el15tcd | 2:8667325e74a9 | 149 | |
el15tcd | 2:8667325e74a9 | 150 | //tile 4 patterns |
el15tcd | 2:8667325e74a9 | 151 | |
el15tcd | 2:8667325e74a9 | 152 | algorithm_update = 1; |
el15tcd | 1:c573caf40864 | 153 | |
el15tcd | 2:8667325e74a9 | 154 | while(algorithm_update == 1) { //set algorithm to 0 to end the algorithm |
el15tcd | 0:8a61e3541a5e | 155 | |
el15tcd | 2:8667325e74a9 | 156 | for (i = 0; i <= 11; i++) { //11 because start bit not guaranteed to be at 0; |
el15tcd | 2:8667325e74a9 | 157 | patterns(i); //turn on pattern(i) |
el15tcd | 2:8667325e74a9 | 158 | int r = detectors(i); //update detectors(i) |
el15tcd | 2:8667325e74a9 | 159 | if (r == 1) { |
el15tcd | 2:8667325e74a9 | 160 | i = 6; |
el15tcd | 2:8667325e74a9 | 161 | r = 0; |
el15tcd | 1:c573caf40864 | 162 | } |
el15tcd | 0:8a61e3541a5e | 163 | } |
el15tcd | 2:8667325e74a9 | 164 | patternsReset(); |
el15tcd | 2:8667325e74a9 | 165 | results(); |
el15tcd | 2:8667325e74a9 | 166 | //output orientation values in desired format |
el15tcd | 2:8667325e74a9 | 167 | |
el15tcd | 0:8a61e3541a5e | 168 | } |
el15tcd | 2:8667325e74a9 | 169 | } |
el15tcd | 0:8a61e3541a5e | 170 | |
el15tcd | 2:8667325e74a9 | 171 | void patterns(int m) { |
el15tcd | 2:8667325e74a9 | 172 | |
el15tcd | 2:8667325e74a9 | 173 | NorthLED = NorthPattern[m]; |
el15tcd | 2:8667325e74a9 | 174 | EastLED = EastPattern[m]; |
el15tcd | 2:8667325e74a9 | 175 | SouthLED = SouthPattern[m]; |
el15tcd | 2:8667325e74a9 | 176 | WestLED = WestPattern[m]; |
el15tcd | 2:8667325e74a9 | 177 | |
el15tcd | 0:8a61e3541a5e | 178 | } |
el15tcd | 0:8a61e3541a5e | 179 | |
el15tcd | 2:8667325e74a9 | 180 | void patternsReset() { |
el15tcd | 2:8667325e74a9 | 181 | |
el15tcd | 2:8667325e74a9 | 182 | NorthLED = 0; |
el15tcd | 2:8667325e74a9 | 183 | EastLED = 0; |
el15tcd | 2:8667325e74a9 | 184 | SouthLED = 0; |
el15tcd | 2:8667325e74a9 | 185 | WestLED = 0; |
el15tcd | 2:8667325e74a9 | 186 | |
el15tcd | 2:8667325e74a9 | 187 | } |
el15tcd | 2:8667325e74a9 | 188 | |
el15tcd | 2:8667325e74a9 | 189 | int detectors(int n) { |
el15tcd | 2:8667325e74a9 | 190 | |
el15tcd | 2:8667325e74a9 | 191 | d = 0; |
el15tcd | 2:8667325e74a9 | 192 | o = n - 6; // |
el15tcd | 2:8667325e74a9 | 193 | |
el15tcd | 2:8667325e74a9 | 194 | NorthDetected[o] = NorthDetect; |
el15tcd | 2:8667325e74a9 | 195 | EastDetected[o] = EastDetect; |
el15tcd | 2:8667325e74a9 | 196 | SouthDetected[o] = SouthDetect; |
el15tcd | 2:8667325e74a9 | 197 | WestDetected[o] = WestDetect; |
el15tcd | 2:8667325e74a9 | 198 | |
el15tcd | 2:8667325e74a9 | 199 | if ((NorthDetected[o] == 1) && (EastDetected[o] == 1) && (SouthDetected[o] == 1) && (WestDetected[o] == 1)) { |
el15tcd | 2:8667325e74a9 | 200 | c++; |
el15tcd | 2:8667325e74a9 | 201 | } else { |
el15tcd | 2:8667325e74a9 | 202 | c = 0; |
el15tcd | 0:8a61e3541a5e | 203 | } |
el15tcd | 2:8667325e74a9 | 204 | |
el15tcd | 2:8667325e74a9 | 205 | if (c == 3) { |
el15tcd | 2:8667325e74a9 | 206 | d = 1; |
el15tcd | 0:8a61e3541a5e | 207 | } |
el15tcd | 2:8667325e74a9 | 208 | |
el15tcd | 2:8667325e74a9 | 209 | return d; |
el15tcd | 2:8667325e74a9 | 210 | |
el15tcd | 0:8a61e3541a5e | 211 | } |
el15tcd | 1:c573caf40864 | 212 | |
el15tcd | 2:8667325e74a9 | 213 | void results() { |
el15tcd | 2:8667325e74a9 | 214 | |
el15tcd | 2:8667325e74a9 | 215 | //compare arrays to the predetermined patterns |
el15tcd | 2:8667325e74a9 | 216 | //cycles through all comparisons of data, comparing the detected values to the known patterns of other tiles |
el15tcd | 2:8667325e74a9 | 217 | //adds a value to the specific counter if it is a match |
el15tcd | 2:8667325e74a9 | 218 | //example |
el15tcd | 2:8667325e74a9 | 219 | for (i = 3; i < 8; i++) {//ignore the start bits when comparing |
el15tcd | 2:8667325e74a9 | 220 | if (NorthDetected[i] == NorthPattern2[i]) { |
el15tcd | 2:8667325e74a9 | 221 | N1N2++; |
el15tcd | 2:8667325e74a9 | 222 | } |
el15tcd | 2:8667325e74a9 | 223 | if (NorthDetected[i] == NorthPattern3[i]) { |
el15tcd | 2:8667325e74a9 | 224 | N1N3++; |
el15tcd | 2:8667325e74a9 | 225 | } |
el15tcd | 2:8667325e74a9 | 226 | if (NorthDetected[i] == NorthPattern4[i]) { |
el15tcd | 2:8667325e74a9 | 227 | N1N4++; |
el15tcd | 2:8667325e74a9 | 228 | } |
el15tcd | 2:8667325e74a9 | 229 | |
el15tcd | 2:8667325e74a9 | 230 | |
el15tcd | 1:c573caf40864 | 231 | } |
el15tcd | 2:8667325e74a9 | 232 | |
el15tcd | 2:8667325e74a9 | 233 | //if any of the variables is 5, you can tell that is the current connection |
el15tcd | 2:8667325e74a9 | 234 | //example |
el15tcd | 2:8667325e74a9 | 235 | if (N1N2 == 5) { |
el15tcd | 2:8667325e74a9 | 236 | tile1_orientation = 1; //always with respect to tile 1 |
el15tcd | 2:8667325e74a9 | 237 | tile2_orientation = 3; |
el15tcd | 2:8667325e74a9 | 238 | } else if (N1N3 == 5) { |
el15tcd | 2:8667325e74a9 | 239 | tile1_orientation = 1; |
el15tcd | 2:8667325e74a9 | 240 | tile3_orientation = 3; |
el15tcd | 2:8667325e74a9 | 241 | } else if (N1N4 == 5) { |
el15tcd | 2:8667325e74a9 | 242 | tile1_orientation = 1; |
el15tcd | 2:8667325e74a9 | 243 | tile4_orientation = 3; |
el15tcd | 1:c573caf40864 | 244 | } |
el15tcd | 2:8667325e74a9 | 245 | |
el15tcd | 2:8667325e74a9 | 246 | if (N1E2 == 5) { |
el15tcd | 2:8667325e74a9 | 247 | tile1_orientation = 1; //always with respect to tile 1 |
el15tcd | 2:8667325e74a9 | 248 | tile2_orientation = 2; |
el15tcd | 2:8667325e74a9 | 249 | } else if (N1E3 == 5) { |
el15tcd | 2:8667325e74a9 | 250 | tile1_orientation = 1; |
el15tcd | 2:8667325e74a9 | 251 | tile3_orientation = 2; |
el15tcd | 2:8667325e74a9 | 252 | } else if (N1E4 == 5) { |
el15tcd | 2:8667325e74a9 | 253 | tile1_orientation = 1; |
el15tcd | 2:8667325e74a9 | 254 | tile4_orientation = 2; |
el15tcd | 1:c573caf40864 | 255 | } |
el15tcd | 1:c573caf40864 | 256 | } |