Todd Dale / Mbed 2 deprecated Orientation_PerTileShape

Shape algorithm for location mapping.

Dependencies:   mbed-rtos mbed

Fork of Orientation_PerTile by Todd Dale

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?

UserRevisionLine numberNew 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 }