Duncan Parker / Mbed OS Accelerometer_example_part3

Part 2

Dependencies:   MMA8451Q

main.cpp@6:b4a7a3e46018, 2019-02-24 (annotated)

Committer:
duncangparker
Date:
Sun Feb 24 14:32:12 2019 +0000
Revision:
6:b4a7a3e46018
Parent:
5:75cd0f7649ca 
Code Tidying

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WilliamMarshQMUL 0:a1caba5c4e48 1 #include "mbed.h"
WilliamMarshQMUL 0:a1caba5c4e48 2 #include "MMA8451Q.h"
WilliamMarshQMUL 0:a1caba5c4e48 3
duncangparker 4:efbc5085138a 4 // rgb (active low)
duncangparker 4:efbc5085138a 5 #define Red (0b0011)
duncangparker 4:efbc5085138a 6 #define Green (0b0101)
duncangparker 4:efbc5085138a 7 #define Blue (0b0110)
duncangparker 4:efbc5085138a 8 #define Yellow (0b0001)
duncangparker 4:efbc5085138a 9 #define Cyan (0b0100)
duncangparker 4:efbc5085138a 10 #define Magenta (0b0010)
duncangparker 4:efbc5085138a 11 #define White (0b0000)
duncangparker 4:efbc5085138a 12 #define Black (0b0111)
duncangparker 4:efbc5085138a 13
duncangparker 4:efbc5085138a 14 #define Xp Red
duncangparker 4:efbc5085138a 15 #define Xn Yellow
duncangparker 4:efbc5085138a 16 #define Yp Green
duncangparker 4:efbc5085138a 17 #define Yn Cyan
duncangparker 4:efbc5085138a 18 #define Zp Blue
duncangparker 4:efbc5085138a 19 #define Zn Magenta
duncangparker 4:efbc5085138a 20 #define IN Black
duncangparker 4:efbc5085138a 21
duncangparker 4:efbc5085138a 22 /*
duncangparker 4:efbc5085138a 23 assuming (x^2 + y^2 + z^2)^0.5 = 1
duncangparker 4:efbc5085138a 24 G-> shared between 2 axes = 0.707 value
duncangparker 4:efbc5085138a 25 G-> shared between 3 axes = 0.577 value
duncangparker 4:efbc5085138a 26 Could make threshold for orientation to be 0.9 giving reasonable margin
duncangparker 4:efbc5085138a 27 */
duncangparker 4:efbc5085138a 28 #define TH 0.9 //Threshold for deciding orientation
duncangparker 4:efbc5085138a 29
duncangparker 4:efbc5085138a 30 // IO declarations
duncangparker 4:efbc5085138a 31 #define MMA8451_I2C_ADDRESS (0x1d<<1)
duncangparker 4:efbc5085138a 32
duncangparker 4:efbc5085138a 33 PinName const SDA = PTE25;
duncangparker 4:efbc5085138a 34 PinName const SCL = PTE24;
WilliamMarshQMUL 0:a1caba5c4e48 35
duncangparker 4:efbc5085138a 36 DigitalOut rled(LED1);
duncangparker 4:efbc5085138a 37 DigitalOut gled(LED2);
duncangparker 4:efbc5085138a 38 DigitalOut bled(LED3);
duncangparker 4:efbc5085138a 39
duncangparker 4:efbc5085138a 40 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
duncangparker 4:efbc5085138a 41 Serial pc(USBTX, USBRX); // tx, rx
duncangparker 4:efbc5085138a 42
duncangparker 4:efbc5085138a 43 int orientation_find(float x, float y, float z){
duncangparker 4:efbc5085138a 44 if (abs(x) > TH){
duncangparker 4:efbc5085138a 45 if (x > 0){
duncangparker 4:efbc5085138a 46 return(Xp);
duncangparker 4:efbc5085138a 47 }
duncangparker 4:efbc5085138a 48 else{
duncangparker 4:efbc5085138a 49 return(Xn);
duncangparker 4:efbc5085138a 50 }
duncangparker 4:efbc5085138a 51 }
duncangparker 4:efbc5085138a 52 else if (abs(y) > TH){
duncangparker 4:efbc5085138a 53 if (y > 0){
duncangparker 4:efbc5085138a 54 return(Yp);
duncangparker 4:efbc5085138a 55 }
duncangparker 4:efbc5085138a 56 else{
duncangparker 4:efbc5085138a 57 return(Yn);
duncangparker 4:efbc5085138a 58 }
duncangparker 4:efbc5085138a 59 }
duncangparker 4:efbc5085138a 60 else if (abs(z) > TH){
duncangparker 4:efbc5085138a 61 if (z > 0){
duncangparker 4:efbc5085138a 62 return(Zp);
duncangparker 4:efbc5085138a 63 }
duncangparker 4:efbc5085138a 64 else{
duncangparker 4:efbc5085138a 65 return(Zn);
duncangparker 4:efbc5085138a 66 }
duncangparker 4:efbc5085138a 67 }
duncangparker 4:efbc5085138a 68 else {
duncangparker 4:efbc5085138a 69 return(IN);
duncangparker 4:efbc5085138a 70 }//end case
duncangparker 4:efbc5085138a 71 }//end func
WilliamMarshQMUL 0:a1caba5c4e48 72
WilliamMarshQMUL 0:a1caba5c4e48 73 int main(void)
WilliamMarshQMUL 0:a1caba5c4e48 74 {
duncangparker 4:efbc5085138a 75 int state = IN;
duncangparker 5:75cd0f7649ca 76 int tick = 0;
duncangparker 5:75cd0f7649ca 77 int system = 1;
WilliamMarshQMUL 0:a1caba5c4e48 78
duncangparker 5:75cd0f7649ca 79 //still needed:
duncangparker 5:75cd0f7649ca 80 pc.printf("MMA8451 ID: %d\n\r", acc.getWhoAmI());
duncangparker 4:efbc5085138a 81
duncangparker 5:75cd0f7649ca 82 //init leds
duncangparker 5:75cd0f7649ca 83 rled = 1;
duncangparker 5:75cd0f7649ca 84 gled = 1;
duncangparker 5:75cd0f7649ca 85 bled = 1;
duncangparker 5:75cd0f7649ca 86
WilliamMarshQMUL 0:a1caba5c4e48 87 while (true) {
WilliamMarshQMUL 0:a1caba5c4e48 88 float x, y, z;
WilliamMarshQMUL 0:a1caba5c4e48 89 x = acc.getAccX();
WilliamMarshQMUL 0:a1caba5c4e48 90 y = acc.getAccY();
WilliamMarshQMUL 0:a1caba5c4e48 91 z = acc.getAccZ();
duncangparker 4:efbc5085138a 92
duncangparker 5:75cd0f7649ca 93 state = orientation_find(x,y,z);
duncangparker 5:75cd0f7649ca 94
duncangparker 5:75cd0f7649ca 95 switch(system){
duncangparker 5:75cd0f7649ca 96 case 1: //initial flat state
duncangparker 5:75cd0f7649ca 97 if(tick > 100 && state == Yn){
duncangparker 6:b4a7a3e46018 98 //pc.printf("passed first stage \n\r");
duncangparker 5:75cd0f7649ca 99 system = 2;
duncangparker 5:75cd0f7649ca 100 tick = 0;
duncangparker 5:75cd0f7649ca 101 }
duncangparker 5:75cd0f7649ca 102 else if( tick < 100 && state == Yn){
duncangparker 5:75cd0f7649ca 103 system = 6;
duncangparker 5:75cd0f7649ca 104 }
duncangparker 5:75cd0f7649ca 105 else if(state != Zp && state != IN){
duncangparker 5:75cd0f7649ca 106 system = 5;
duncangparker 4:efbc5085138a 107 }
duncangparker 4:efbc5085138a 108 break;
duncangparker 4:efbc5085138a 109
duncangparker 5:75cd0f7649ca 110 case 2: //right orientation state
duncangparker 5:75cd0f7649ca 111 if(tick > 20 && tick < 60 && state == Xn){
duncangparker 6:b4a7a3e46018 112 //pc.printf("passed second stage \n\r");
duncangparker 5:75cd0f7649ca 113 system = 3;
duncangparker 5:75cd0f7649ca 114 tick = 0;
duncangparker 5:75cd0f7649ca 115 }
duncangparker 5:75cd0f7649ca 116 else if((tick > 20 && state == Xn) || tick > 60 ){
duncangparker 5:75cd0f7649ca 117 system = 6;
duncangparker 5:75cd0f7649ca 118 }
duncangparker 5:75cd0f7649ca 119 else if(state != Yn && state != IN){
duncangparker 5:75cd0f7649ca 120 system = 5;
duncangparker 5:75cd0f7649ca 121 }
duncangparker 5:75cd0f7649ca 122 break;
duncangparker 5:75cd0f7649ca 123
duncangparker 5:75cd0f7649ca 124 case 3: //upwards orientation
duncangparker 5:75cd0f7649ca 125 if(tick > 40 && tick < 80 && state == Zp){
duncangparker 6:b4a7a3e46018 126 //pc.printf("passed third stage \n\r");
duncangparker 5:75cd0f7649ca 127 system = 4;
duncangparker 5:75cd0f7649ca 128 tick = 0;
duncangparker 5:75cd0f7649ca 129 }
duncangparker 5:75cd0f7649ca 130 else if((tick > 40 && state == Zp) || tick > 80 ){
duncangparker 5:75cd0f7649ca 131 system = 6;
duncangparker 5:75cd0f7649ca 132 }
duncangparker 5:75cd0f7649ca 133 else if(state != Xn && state != IN){
duncangparker 5:75cd0f7649ca 134 system = 5;
duncangparker 4:efbc5085138a 135 }
duncangparker 5:75cd0f7649ca 136 break;
duncangparker 5:75cd0f7649ca 137
duncangparker 5:75cd0f7649ca 138 case 4://flat success
duncangparker 5:75cd0f7649ca 139 pc.printf("Sequence is complete\n\r");
duncangparker 5:75cd0f7649ca 140 system = 7;
duncangparker 5:75cd0f7649ca 141 gled = 0;
duncangparker 5:75cd0f7649ca 142 break;
duncangparker 5:75cd0f7649ca 143
duncangparker 5:75cd0f7649ca 144 case 5://sequence fail
duncangparker 5:75cd0f7649ca 145 pc.printf("Sequence Error \n\r");
duncangparker 5:75cd0f7649ca 146 system = 7;
duncangparker 5:75cd0f7649ca 147 rled = 0;
duncangparker 5:75cd0f7649ca 148 break;
duncangparker 5:75cd0f7649ca 149
duncangparker 5:75cd0f7649ca 150 case 6://timing fail
duncangparker 5:75cd0f7649ca 151 pc.printf("Time error \n\r");
duncangparker 5:75cd0f7649ca 152 system = 7;
duncangparker 5:75cd0f7649ca 153 rled = 0;
duncangparker 5:75cd0f7649ca 154 break;
duncangparker 5:75cd0f7649ca 155
duncangparker 5:75cd0f7649ca 156 case 7://idle state
duncangparker 5:75cd0f7649ca 157 // nothing happens
duncangparker 5:75cd0f7649ca 158 break;
duncangparker 4:efbc5085138a 159
duncangparker 5:75cd0f7649ca 160 }// end switch
duncangparker 5:75cd0f7649ca 161 wait(0.1); // 100ms timing accuracy,
duncangparker 5:75cd0f7649ca 162 tick++;
duncangparker 4:efbc5085138a 163 }//end while
duncangparker 4:efbc5085138a 164 }//end main