Andrew Mkpanam
/
Accelerometer_pt2
Final
Diff: main.cpp
- Revision:
- 6:65d8a80eb0b6
- Parent:
- 4:72b8fb7423dd
--- a/main.cpp Thu Feb 27 10:07:17 2020 +0000 +++ b/main.cpp Thu Mar 05 23:04:02 2020 +0000 @@ -5,7 +5,32 @@ PinName const SCL = PTE24; #define MMA8451_I2C_ADDRESS (0x1d<<1) - +int getOrientation(float x, float y, float z) +{ + int retval = 6; + if (z > 0.9) // State = Flat + { + retval = 0; + } else if (x < -0.9) //state is up + { + retval = 1; + } else if (y < -0.9) //State = Right + { + + retval = 2; + } else if (y > 0.9) // state is left + { + retval = 3; + } else if (x > 0.9) //state is Down + { + retval = 4; + } else if (z < -0.9) //state is over + { + retval = 5; + } + + return retval; +} int main(void) { MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); @@ -13,19 +38,309 @@ PwmOut gled(LED2); PwmOut bled(LED3); Serial pc(USBTX, USBRX); // tx, rx + enum states {FLAT, UP, RIGHT, LEFT, DOWN, OVER, INTERMEDIATE}; + + int devFlat = 0; + int devUp = 0; + int devRight = 0; + bool sequence = true; + bool trigger = false; + float elapsed = 0; + Timer t; + t.start(); //incase t.read() throws an error in the first loop + pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI()); while (true) { float x, y, z; + int devState = 0; x = acc.getAccX(); y = acc.getAccY(); z = acc.getAccZ(); - rled = 1.0f - abs(x); + + //enum states deviceState; + devState = getOrientation(x,y,z); + + if (sequence == false ) + { + pc.printf("Invalid Sequence. Sequence is broken \n\r"); + if (devState == FLAT) { + t.stop(); + elapsed = t.read(); + if (elapsed <= 4) { + pc.printf("Sequence is broken. Keep flat for 10 Seconds to reset %f\n\r", elapsed); + t.start(); + } else { + pc.printf("Resetting is complete %f\n\r", elapsed); + sequence = true; + t.reset(); + t.start(); + devUp = 0; + devRight = 0; + devFlat = 0; + } + } else { + t.stop(); + elapsed = t.read(); + pc.printf("Device must be flat for 10 seconds, %f \n", elapsed); + t.reset(); + t.start(); + sequence = false; + } + } + else + { + if ((devFlat == 0) && (devUp == 0) && (devRight == 0)) //Check if device is initialized with no state + { + if (devState == FLAT) { //Check if device is flat + t.stop(); + elapsed = t.read(); + if (elapsed < 10 ) { // ... check if it is still under 10 secs + sequence = true; + t.start(); + pc.printf("Correct State. State is FLAT Time is < 10 SECS %d %f\n\r", devState, elapsed); + rled = 1; + gled = 0; + bled = 1; + } else { + pc.printf("10 seconds have elapsed, expect transition %d %f\n\r", devState, elapsed); + devFlat = 1; // 10 secs has elapsed, move to the next state + t.start(); + } + } else if (devState == RIGHT) { // state has changed to right + t.stop(); + elapsed = t.read(); + if (elapsed > 10 ) // It is over 10 seconds + { + pc.printf("10 seconds have elapsed, State is now right %d %f\n\r", devState, elapsed); + sequence = true; + devFlat = 1; + t.reset(); + t.start(); + } else { // State changed to right before 10 secs elapsed + sequence = false; + pc.printf("Incorrect state. Turned right too early, State is now right %d %f\n\r", devState, elapsed); + t.stop(); + t.reset(); + t.start(); + } + + } + else if (devState == INTERMEDIATE) { + pc.printf("Intermediate State. %f\n\r", elapsed); + sequence = true; + } + else { //This is the invalid state in this case. + sequence = false; + t.stop(); + elapsed = t.read(); + pc.printf("Invalid State, State should be FLAT or RIGHT after 10 secs %f\n\r", elapsed); + t.reset(); + t.start(); + } + } else if ((devFlat == 1) && (devRight == 0) && (devUp == 0)) //check that the previous state was flat + { + if (devState == RIGHT) // Device is in the correct state. + { + t.stop(); + elapsed = t.read(); + if (elapsed > 10) { // ... but check if it entered this state after the elapsed time + sequence = true; + t.reset(); // reset the previous timer + t.start(); + pc.printf("Correct State. State is RIGHT TIME IS > 10 SECS %d %f\n\r", devState, elapsed); + } else if ((elapsed > 2) && (elapsed <= 6)) { + t.start(); + pc.printf("Correct State. State is RIGHT: %d, Time is %f \n\r", devState, elapsed); + sequence = true; + } else if (elapsed < 2) { // still in right, do nothing + t.start(); + rled = 1; + gled = 1; + bled = 1; + pc.printf("Correct State. State is RIGHT, Time is %f \n\r", elapsed); + sequence = true; + } else if (elapsed > 6) { + devRight = 1; + t.reset(); + t.start(); + sequence = true; + pc.printf("Correct State. Expecting new state. time is %f \n\r", elapsed); + } + + } else if (devState == FLAT) //it will never enter this state + { + t.stop(); + elapsed = t.read(); + + if (elapsed > 10 ) { //check if it is within the expected duration + sequence = true; + pc.printf("Correct State. State is still Flat: %d, However TIME > 10 SECS Timer is %f \n\r", devState, elapsed); + rled = 1; + gled = 0; + bled = 1; + t.start(); + } else { + 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); + rled = 0; + gled = 1; + bled = 1; + t.reset(); + t.start(); + sequence = false; + } + } + else if (devState == INTERMEDIATE) { + pc.printf("Intermediate State. %f\n\r", elapsed); + sequence = true; + } + else { //Device is in the wrong state + pc.printf("Inorrect State. State should be flat or right: %d, and TIME < 10 SECS Timer is %f \n\r", devState, elapsed); + t.stop(); + t.reset(); + t.start(); + sequence = false; + rled = 0; + gled = 1; + bled = 1; + } + } else if ((devFlat == 1) && (devRight == 1) && (devUp == 0)) //DO THE SAME THING FOR THIS ONE + { + if (devState == UP) // Device is in the correct state + { + t.stop(); + elapsed = t.read(); + if (elapsed < 9) { // ... but check if it entered this state after the elapsed time (fix this one o) + t.start(); + rled = 1; + gled = 1; + bled = 1; + pc.printf("Correct State. State is UP %d and time is %f \n\r", devState, elapsed); + sequence = true; + } else { + t.reset(); + t.start(); + devUp = 1; + sequence = true; + pc.printf("Sequence Complete! %f\n\r", elapsed); + } + + } + else if (devState == RIGHT) { + t.stop(); + elapsed = t.read(); + if (elapsed <= 3) { + pc.printf("Go up Now. %f\n\r", elapsed); + t.start(); + sequence = true; + } else { + sequence = false; + t.reset(); + t.start(); + pc.printf("Cannot help you. Should have gone up. %f\n\r", elapsed); + } + } + else if (devState == INTERMEDIATE) { + pc.printf("Intermediate State. %f\n\r", elapsed); + sequence = true; + } + else + { + rled = 0; + gled = 1; + bled = 1; + + t.stop(); + elapsed = t.read(); + if ((elapsed < 4) ) { + pc.printf("Incorrect State. State is NOT UP %d and time is %f \n\r", devState, elapsed); // its entering this state + t.reset(); + sequence = false; + t.start(); + + } else { + pc.printf("WHERE AM I %d and time is %f \n\r", devState, elapsed); + t.reset(); + t.start(); + } + + } + } else if ((devFlat == 1) && (devRight == 1) && (devUp == 1)) //Last State + { + if (devState == FLAT) // Device is flat (correct state) + { + t.stop(); + elapsed = t.read(); + if (elapsed < 10) { // ... if time is less than 10 secs, do nothing + t.start(); + pc.printf("Correct State Device should be flat for 10 secs, %f\n\r", elapsed); + sequence = true; + } else { // if time is >= 10 seconds do trigger reset all states + pc.printf("Trigger Activated. State is %d \n\r", devState); + trigger = true; + t.reset(); + t.start(); + sequence = true; + devFlat = 0; + devUp = 0; + devRight = 0; + } + + } + else if (devState == INTERMEDIATE) { + pc.printf("Intermediate State. %f\n\r", elapsed); + sequence = true; + } + else //device is not flat, show error + { + t.stop(); + elapsed = t.read(); + t.reset(); + pc.printf("Device should be flat for 10 seconds. State is %d, timer is %f \n\r", devState, elapsed); + // measure time. If time has elapsed, sequence is false else sequence is true + } + } + else + { + t.stop(); + t.start(); + sequence = false; + } + } + + if (sequence == true) + { + // pc.printf("Sequence Correct %d, Timer: %f\n\r", devState, elapsed); + } else { + // pc.printf("Sequence False %d, Timer: %f\n\r", devState, elapsed); + } + + if (trigger == true) { //do trigger thing + } + + + ThisThread::sleep_for(300); + + /* if (orientation == flat) + { + pc.printf("flat\n"); + } else if (orientation == up) + { + pc.printf("up\n"); + } else if (orientation == right) + { + pc.printf("right\n"); + } else { + pc.printf("Unknown State\n"); + }*/ + + /*rled = 1.0f - abs(x); gled = 1.0f - abs(y); - bled = 1.0f - abs(z); - ThisThread::sleep_for(300); // wait(0.3); - pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z); + bled = 1.0f - abs(z);*/ + + //ThisThread::sleep_for(300); // wait(0.3); + //pc.printf("X: %1.2f, Y: %1.2f, Z: %1.2f\n", x, y, z); } }