Andrew Mkpanam
/
Accelerometer_pt2
Final
main.cpp@6:65d8a80eb0b6, 2020-03-05 (annotated)
- Committer:
- ex19397
- Date:
- Thu Mar 05 23:04:02 2020 +0000
- Revision:
- 6:65d8a80eb0b6
- Parent:
- 4:72b8fb7423dd
Final
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WilliamMarshQMUL | 0:a1caba5c4e48 | 1 | #include "mbed.h" |
WilliamMarshQMUL | 0:a1caba5c4e48 | 2 | #include "MMA8451Q.h" |
WilliamMarshQMUL | 0:a1caba5c4e48 | 3 | |
WilliamMarshQMUL | 0:a1caba5c4e48 | 4 | PinName const SDA = PTE25; |
WilliamMarshQMUL | 0:a1caba5c4e48 | 5 | PinName const SCL = PTE24; |
WilliamMarshQMUL | 0:a1caba5c4e48 | 6 | |
WilliamMarshQMUL | 0:a1caba5c4e48 | 7 | #define MMA8451_I2C_ADDRESS (0x1d<<1) |
ex19397 | 6:65d8a80eb0b6 | 8 | int getOrientation(float x, float y, float z) |
ex19397 | 6:65d8a80eb0b6 | 9 | { |
ex19397 | 6:65d8a80eb0b6 | 10 | int retval = 6; |
ex19397 | 6:65d8a80eb0b6 | 11 | if (z > 0.9) // State = Flat |
ex19397 | 6:65d8a80eb0b6 | 12 | { |
ex19397 | 6:65d8a80eb0b6 | 13 | retval = 0; |
ex19397 | 6:65d8a80eb0b6 | 14 | } else if (x < -0.9) //state is up |
ex19397 | 6:65d8a80eb0b6 | 15 | { |
ex19397 | 6:65d8a80eb0b6 | 16 | retval = 1; |
ex19397 | 6:65d8a80eb0b6 | 17 | } else if (y < -0.9) //State = Right |
ex19397 | 6:65d8a80eb0b6 | 18 | { |
ex19397 | 6:65d8a80eb0b6 | 19 | |
ex19397 | 6:65d8a80eb0b6 | 20 | retval = 2; |
ex19397 | 6:65d8a80eb0b6 | 21 | } else if (y > 0.9) // state is left |
ex19397 | 6:65d8a80eb0b6 | 22 | { |
ex19397 | 6:65d8a80eb0b6 | 23 | retval = 3; |
ex19397 | 6:65d8a80eb0b6 | 24 | } else if (x > 0.9) //state is Down |
ex19397 | 6:65d8a80eb0b6 | 25 | { |
ex19397 | 6:65d8a80eb0b6 | 26 | retval = 4; |
ex19397 | 6:65d8a80eb0b6 | 27 | } else if (z < -0.9) //state is over |
ex19397 | 6:65d8a80eb0b6 | 28 | { |
ex19397 | 6:65d8a80eb0b6 | 29 | retval = 5; |
ex19397 | 6:65d8a80eb0b6 | 30 | } |
ex19397 | 6:65d8a80eb0b6 | 31 | |
ex19397 | 6:65d8a80eb0b6 | 32 | return retval; |
ex19397 | 6:65d8a80eb0b6 | 33 | } |
WilliamMarshQMUL | 0:a1caba5c4e48 | 34 | int main(void) |
WilliamMarshQMUL | 0:a1caba5c4e48 | 35 | { |
WilliamMarshQMUL | 0:a1caba5c4e48 | 36 | MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 37 | PwmOut rled(LED1); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 38 | PwmOut gled(LED2); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 39 | PwmOut bled(LED3); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 40 | Serial pc(USBTX, USBRX); // tx, rx |
ex19397 | 6:65d8a80eb0b6 | 41 | enum states {FLAT, UP, RIGHT, LEFT, DOWN, OVER, INTERMEDIATE}; |
ex19397 | 6:65d8a80eb0b6 | 42 | |
ex19397 | 6:65d8a80eb0b6 | 43 | int devFlat = 0; |
ex19397 | 6:65d8a80eb0b6 | 44 | int devUp = 0; |
ex19397 | 6:65d8a80eb0b6 | 45 | int devRight = 0; |
ex19397 | 6:65d8a80eb0b6 | 46 | bool sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 47 | bool trigger = false; |
ex19397 | 6:65d8a80eb0b6 | 48 | float elapsed = 0; |
ex19397 | 6:65d8a80eb0b6 | 49 | Timer t; |
ex19397 | 6:65d8a80eb0b6 | 50 | t.start(); //incase t.read() throws an error in the first loop |
ex19397 | 6:65d8a80eb0b6 | 51 | |
WilliamMarshQMUL | 0:a1caba5c4e48 | 52 | |
WilliamMarshQMUL | 0:a1caba5c4e48 | 53 | |
WilliamMarshQMUL | 0:a1caba5c4e48 | 54 | pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI()); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 55 | |
WilliamMarshQMUL | 0:a1caba5c4e48 | 56 | while (true) { |
WilliamMarshQMUL | 0:a1caba5c4e48 | 57 | float x, y, z; |
ex19397 | 6:65d8a80eb0b6 | 58 | int devState = 0; |
WilliamMarshQMUL | 0:a1caba5c4e48 | 59 | x = acc.getAccX(); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 60 | y = acc.getAccY(); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 61 | z = acc.getAccZ(); |
ex19397 | 6:65d8a80eb0b6 | 62 | |
ex19397 | 6:65d8a80eb0b6 | 63 | //enum states deviceState; |
ex19397 | 6:65d8a80eb0b6 | 64 | devState = getOrientation(x,y,z); |
ex19397 | 6:65d8a80eb0b6 | 65 | |
ex19397 | 6:65d8a80eb0b6 | 66 | if (sequence == false ) |
ex19397 | 6:65d8a80eb0b6 | 67 | { |
ex19397 | 6:65d8a80eb0b6 | 68 | pc.printf("Invalid Sequence. Sequence is broken \n\r"); |
ex19397 | 6:65d8a80eb0b6 | 69 | if (devState == FLAT) { |
ex19397 | 6:65d8a80eb0b6 | 70 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 71 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 72 | if (elapsed <= 4) { |
ex19397 | 6:65d8a80eb0b6 | 73 | pc.printf("Sequence is broken. Keep flat for 10 Seconds to reset %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 74 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 75 | } else { |
ex19397 | 6:65d8a80eb0b6 | 76 | pc.printf("Resetting is complete %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 77 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 78 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 79 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 80 | devUp = 0; |
ex19397 | 6:65d8a80eb0b6 | 81 | devRight = 0; |
ex19397 | 6:65d8a80eb0b6 | 82 | devFlat = 0; |
ex19397 | 6:65d8a80eb0b6 | 83 | } |
ex19397 | 6:65d8a80eb0b6 | 84 | } else { |
ex19397 | 6:65d8a80eb0b6 | 85 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 86 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 87 | pc.printf("Device must be flat for 10 seconds, %f \n", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 88 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 89 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 90 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 91 | } |
ex19397 | 6:65d8a80eb0b6 | 92 | } |
ex19397 | 6:65d8a80eb0b6 | 93 | else |
ex19397 | 6:65d8a80eb0b6 | 94 | { |
ex19397 | 6:65d8a80eb0b6 | 95 | if ((devFlat == 0) && (devUp == 0) && (devRight == 0)) //Check if device is initialized with no state |
ex19397 | 6:65d8a80eb0b6 | 96 | { |
ex19397 | 6:65d8a80eb0b6 | 97 | if (devState == FLAT) { //Check if device is flat |
ex19397 | 6:65d8a80eb0b6 | 98 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 99 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 100 | if (elapsed < 10 ) { // ... check if it is still under 10 secs |
ex19397 | 6:65d8a80eb0b6 | 101 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 102 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 103 | pc.printf("Correct State. State is FLAT Time is < 10 SECS %d %f\n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 104 | rled = 1; |
ex19397 | 6:65d8a80eb0b6 | 105 | gled = 0; |
ex19397 | 6:65d8a80eb0b6 | 106 | bled = 1; |
ex19397 | 6:65d8a80eb0b6 | 107 | } else { |
ex19397 | 6:65d8a80eb0b6 | 108 | pc.printf("10 seconds have elapsed, expect transition %d %f\n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 109 | devFlat = 1; // 10 secs has elapsed, move to the next state |
ex19397 | 6:65d8a80eb0b6 | 110 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 111 | } |
ex19397 | 6:65d8a80eb0b6 | 112 | } else if (devState == RIGHT) { // state has changed to right |
ex19397 | 6:65d8a80eb0b6 | 113 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 114 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 115 | if (elapsed > 10 ) // It is over 10 seconds |
ex19397 | 6:65d8a80eb0b6 | 116 | { |
ex19397 | 6:65d8a80eb0b6 | 117 | pc.printf("10 seconds have elapsed, State is now right %d %f\n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 118 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 119 | devFlat = 1; |
ex19397 | 6:65d8a80eb0b6 | 120 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 121 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 122 | } else { // State changed to right before 10 secs elapsed |
ex19397 | 6:65d8a80eb0b6 | 123 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 124 | pc.printf("Incorrect state. Turned right too early, State is now right %d %f\n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 125 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 126 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 127 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 128 | } |
ex19397 | 6:65d8a80eb0b6 | 129 | |
ex19397 | 6:65d8a80eb0b6 | 130 | } |
ex19397 | 6:65d8a80eb0b6 | 131 | else if (devState == INTERMEDIATE) { |
ex19397 | 6:65d8a80eb0b6 | 132 | pc.printf("Intermediate State. %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 133 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 134 | } |
ex19397 | 6:65d8a80eb0b6 | 135 | else { //This is the invalid state in this case. |
ex19397 | 6:65d8a80eb0b6 | 136 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 137 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 138 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 139 | pc.printf("Invalid State, State should be FLAT or RIGHT after 10 secs %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 140 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 141 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 142 | } |
ex19397 | 6:65d8a80eb0b6 | 143 | } else if ((devFlat == 1) && (devRight == 0) && (devUp == 0)) //check that the previous state was flat |
ex19397 | 6:65d8a80eb0b6 | 144 | { |
ex19397 | 6:65d8a80eb0b6 | 145 | if (devState == RIGHT) // Device is in the correct state. |
ex19397 | 6:65d8a80eb0b6 | 146 | { |
ex19397 | 6:65d8a80eb0b6 | 147 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 148 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 149 | if (elapsed > 10) { // ... but check if it entered this state after the elapsed time |
ex19397 | 6:65d8a80eb0b6 | 150 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 151 | t.reset(); // reset the previous timer |
ex19397 | 6:65d8a80eb0b6 | 152 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 153 | pc.printf("Correct State. State is RIGHT TIME IS > 10 SECS %d %f\n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 154 | } else if ((elapsed > 2) && (elapsed <= 6)) { |
ex19397 | 6:65d8a80eb0b6 | 155 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 156 | pc.printf("Correct State. State is RIGHT: %d, Time is %f \n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 157 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 158 | } else if (elapsed < 2) { // still in right, do nothing |
ex19397 | 6:65d8a80eb0b6 | 159 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 160 | rled = 1; |
ex19397 | 6:65d8a80eb0b6 | 161 | gled = 1; |
ex19397 | 6:65d8a80eb0b6 | 162 | bled = 1; |
ex19397 | 6:65d8a80eb0b6 | 163 | pc.printf("Correct State. State is RIGHT, Time is %f \n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 164 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 165 | } else if (elapsed > 6) { |
ex19397 | 6:65d8a80eb0b6 | 166 | devRight = 1; |
ex19397 | 6:65d8a80eb0b6 | 167 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 168 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 169 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 170 | pc.printf("Correct State. Expecting new state. time is %f \n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 171 | } |
ex19397 | 6:65d8a80eb0b6 | 172 | |
ex19397 | 6:65d8a80eb0b6 | 173 | } else if (devState == FLAT) //it will never enter this state |
ex19397 | 6:65d8a80eb0b6 | 174 | { |
ex19397 | 6:65d8a80eb0b6 | 175 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 176 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 177 | |
ex19397 | 6:65d8a80eb0b6 | 178 | if (elapsed > 10 ) { //check if it is within the expected duration |
ex19397 | 6:65d8a80eb0b6 | 179 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 180 | pc.printf("Correct State. State is still Flat: %d, However TIME > 10 SECS Timer is %f \n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 181 | rled = 1; |
ex19397 | 6:65d8a80eb0b6 | 182 | gled = 0; |
ex19397 | 6:65d8a80eb0b6 | 183 | bled = 1; |
ex19397 | 6:65d8a80eb0b6 | 184 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 185 | } else { |
ex19397 | 6:65d8a80eb0b6 | 186 | pc.printf("Incorrect State. I shouldn't even be here. State is RIGHT: %d, BUT TIME < 10 SECS Timer is %f \n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 187 | rled = 0; |
ex19397 | 6:65d8a80eb0b6 | 188 | gled = 1; |
ex19397 | 6:65d8a80eb0b6 | 189 | bled = 1; |
ex19397 | 6:65d8a80eb0b6 | 190 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 191 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 192 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 193 | } |
ex19397 | 6:65d8a80eb0b6 | 194 | } |
ex19397 | 6:65d8a80eb0b6 | 195 | else if (devState == INTERMEDIATE) { |
ex19397 | 6:65d8a80eb0b6 | 196 | pc.printf("Intermediate State. %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 197 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 198 | } |
ex19397 | 6:65d8a80eb0b6 | 199 | else { //Device is in the wrong state |
ex19397 | 6:65d8a80eb0b6 | 200 | pc.printf("Inorrect State. State should be flat or right: %d, and TIME < 10 SECS Timer is %f \n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 201 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 202 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 203 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 204 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 205 | rled = 0; |
ex19397 | 6:65d8a80eb0b6 | 206 | gled = 1; |
ex19397 | 6:65d8a80eb0b6 | 207 | bled = 1; |
ex19397 | 6:65d8a80eb0b6 | 208 | } |
ex19397 | 6:65d8a80eb0b6 | 209 | } else if ((devFlat == 1) && (devRight == 1) && (devUp == 0)) //DO THE SAME THING FOR THIS ONE |
ex19397 | 6:65d8a80eb0b6 | 210 | { |
ex19397 | 6:65d8a80eb0b6 | 211 | if (devState == UP) // Device is in the correct state |
ex19397 | 6:65d8a80eb0b6 | 212 | { |
ex19397 | 6:65d8a80eb0b6 | 213 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 214 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 215 | if (elapsed < 9) { // ... but check if it entered this state after the elapsed time (fix this one o) |
ex19397 | 6:65d8a80eb0b6 | 216 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 217 | rled = 1; |
ex19397 | 6:65d8a80eb0b6 | 218 | gled = 1; |
ex19397 | 6:65d8a80eb0b6 | 219 | bled = 1; |
ex19397 | 6:65d8a80eb0b6 | 220 | pc.printf("Correct State. State is UP %d and time is %f \n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 221 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 222 | } else { |
ex19397 | 6:65d8a80eb0b6 | 223 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 224 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 225 | devUp = 1; |
ex19397 | 6:65d8a80eb0b6 | 226 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 227 | pc.printf("Sequence Complete! %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 228 | } |
ex19397 | 6:65d8a80eb0b6 | 229 | |
ex19397 | 6:65d8a80eb0b6 | 230 | } |
ex19397 | 6:65d8a80eb0b6 | 231 | else if (devState == RIGHT) { |
ex19397 | 6:65d8a80eb0b6 | 232 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 233 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 234 | if (elapsed <= 3) { |
ex19397 | 6:65d8a80eb0b6 | 235 | pc.printf("Go up Now. %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 236 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 237 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 238 | } else { |
ex19397 | 6:65d8a80eb0b6 | 239 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 240 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 241 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 242 | pc.printf("Cannot help you. Should have gone up. %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 243 | } |
ex19397 | 6:65d8a80eb0b6 | 244 | } |
ex19397 | 6:65d8a80eb0b6 | 245 | else if (devState == INTERMEDIATE) { |
ex19397 | 6:65d8a80eb0b6 | 246 | pc.printf("Intermediate State. %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 247 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 248 | } |
ex19397 | 6:65d8a80eb0b6 | 249 | else |
ex19397 | 6:65d8a80eb0b6 | 250 | { |
ex19397 | 6:65d8a80eb0b6 | 251 | rled = 0; |
ex19397 | 6:65d8a80eb0b6 | 252 | gled = 1; |
ex19397 | 6:65d8a80eb0b6 | 253 | bled = 1; |
ex19397 | 6:65d8a80eb0b6 | 254 | |
ex19397 | 6:65d8a80eb0b6 | 255 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 256 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 257 | if ((elapsed < 4) ) { |
ex19397 | 6:65d8a80eb0b6 | 258 | pc.printf("Incorrect State. State is NOT UP %d and time is %f \n\r", devState, elapsed); // its entering this state |
ex19397 | 6:65d8a80eb0b6 | 259 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 260 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 261 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 262 | |
ex19397 | 6:65d8a80eb0b6 | 263 | } else { |
ex19397 | 6:65d8a80eb0b6 | 264 | pc.printf("WHERE AM I %d and time is %f \n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 265 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 266 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 267 | } |
ex19397 | 6:65d8a80eb0b6 | 268 | |
ex19397 | 6:65d8a80eb0b6 | 269 | } |
ex19397 | 6:65d8a80eb0b6 | 270 | } else if ((devFlat == 1) && (devRight == 1) && (devUp == 1)) //Last State |
ex19397 | 6:65d8a80eb0b6 | 271 | { |
ex19397 | 6:65d8a80eb0b6 | 272 | if (devState == FLAT) // Device is flat (correct state) |
ex19397 | 6:65d8a80eb0b6 | 273 | { |
ex19397 | 6:65d8a80eb0b6 | 274 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 275 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 276 | if (elapsed < 10) { // ... if time is less than 10 secs, do nothing |
ex19397 | 6:65d8a80eb0b6 | 277 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 278 | pc.printf("Correct State Device should be flat for 10 secs, %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 279 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 280 | } else { // if time is >= 10 seconds do trigger reset all states |
ex19397 | 6:65d8a80eb0b6 | 281 | pc.printf("Trigger Activated. State is %d \n\r", devState); |
ex19397 | 6:65d8a80eb0b6 | 282 | trigger = true; |
ex19397 | 6:65d8a80eb0b6 | 283 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 284 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 285 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 286 | devFlat = 0; |
ex19397 | 6:65d8a80eb0b6 | 287 | devUp = 0; |
ex19397 | 6:65d8a80eb0b6 | 288 | devRight = 0; |
ex19397 | 6:65d8a80eb0b6 | 289 | } |
ex19397 | 6:65d8a80eb0b6 | 290 | |
ex19397 | 6:65d8a80eb0b6 | 291 | } |
ex19397 | 6:65d8a80eb0b6 | 292 | else if (devState == INTERMEDIATE) { |
ex19397 | 6:65d8a80eb0b6 | 293 | pc.printf("Intermediate State. %f\n\r", elapsed); |
ex19397 | 6:65d8a80eb0b6 | 294 | sequence = true; |
ex19397 | 6:65d8a80eb0b6 | 295 | } |
ex19397 | 6:65d8a80eb0b6 | 296 | else //device is not flat, show error |
ex19397 | 6:65d8a80eb0b6 | 297 | { |
ex19397 | 6:65d8a80eb0b6 | 298 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 299 | elapsed = t.read(); |
ex19397 | 6:65d8a80eb0b6 | 300 | t.reset(); |
ex19397 | 6:65d8a80eb0b6 | 301 | pc.printf("Device should be flat for 10 seconds. State is %d, timer is %f \n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 302 | // measure time. If time has elapsed, sequence is false else sequence is true |
ex19397 | 6:65d8a80eb0b6 | 303 | } |
ex19397 | 6:65d8a80eb0b6 | 304 | } |
ex19397 | 6:65d8a80eb0b6 | 305 | else |
ex19397 | 6:65d8a80eb0b6 | 306 | { |
ex19397 | 6:65d8a80eb0b6 | 307 | t.stop(); |
ex19397 | 6:65d8a80eb0b6 | 308 | t.start(); |
ex19397 | 6:65d8a80eb0b6 | 309 | sequence = false; |
ex19397 | 6:65d8a80eb0b6 | 310 | } |
ex19397 | 6:65d8a80eb0b6 | 311 | } |
ex19397 | 6:65d8a80eb0b6 | 312 | |
ex19397 | 6:65d8a80eb0b6 | 313 | if (sequence == true) |
ex19397 | 6:65d8a80eb0b6 | 314 | { |
ex19397 | 6:65d8a80eb0b6 | 315 | // pc.printf("Sequence Correct %d, Timer: %f\n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 316 | } else { |
ex19397 | 6:65d8a80eb0b6 | 317 | // pc.printf("Sequence False %d, Timer: %f\n\r", devState, elapsed); |
ex19397 | 6:65d8a80eb0b6 | 318 | } |
ex19397 | 6:65d8a80eb0b6 | 319 | |
ex19397 | 6:65d8a80eb0b6 | 320 | if (trigger == true) { //do trigger thing |
ex19397 | 6:65d8a80eb0b6 | 321 | } |
ex19397 | 6:65d8a80eb0b6 | 322 | |
ex19397 | 6:65d8a80eb0b6 | 323 | |
ex19397 | 6:65d8a80eb0b6 | 324 | ThisThread::sleep_for(300); |
ex19397 | 6:65d8a80eb0b6 | 325 | |
ex19397 | 6:65d8a80eb0b6 | 326 | /* if (orientation == flat) |
ex19397 | 6:65d8a80eb0b6 | 327 | { |
ex19397 | 6:65d8a80eb0b6 | 328 | pc.printf("flat\n"); |
ex19397 | 6:65d8a80eb0b6 | 329 | } else if (orientation == up) |
ex19397 | 6:65d8a80eb0b6 | 330 | { |
ex19397 | 6:65d8a80eb0b6 | 331 | pc.printf("up\n"); |
ex19397 | 6:65d8a80eb0b6 | 332 | } else if (orientation == right) |
ex19397 | 6:65d8a80eb0b6 | 333 | { |
ex19397 | 6:65d8a80eb0b6 | 334 | pc.printf("right\n"); |
ex19397 | 6:65d8a80eb0b6 | 335 | } else { |
ex19397 | 6:65d8a80eb0b6 | 336 | pc.printf("Unknown State\n"); |
ex19397 | 6:65d8a80eb0b6 | 337 | }*/ |
ex19397 | 6:65d8a80eb0b6 | 338 | |
ex19397 | 6:65d8a80eb0b6 | 339 | /*rled = 1.0f - abs(x); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 340 | gled = 1.0f - abs(y); |
ex19397 | 6:65d8a80eb0b6 | 341 | bled = 1.0f - abs(z);*/ |
ex19397 | 6:65d8a80eb0b6 | 342 | |
ex19397 | 6:65d8a80eb0b6 | 343 | //ThisThread::sleep_for(300); // wait(0.3); |
ex19397 | 6:65d8a80eb0b6 | 344 | //pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z); |
WilliamMarshQMUL | 0:a1caba5c4e48 | 345 | } |
WilliamMarshQMUL | 0:a1caba5c4e48 | 346 | } |