Duncan Parker / Mbed OS Accelerometer_example_part3

Part 2

Dependencies:   MMA8451Q

main.cpp@5:75cd0f7649ca, 2019-02-20 (annotated)

Committer:
duncangparker
Date:
Wed Feb 20 18:28:12 2019 +0000
Revision:
5:75cd0f7649ca
Parent:
4:efbc5085138a
Child:
6:b4a7a3e46018
Code working and tidied

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
duncangparker 5:75cd0f7649ca 87
WilliamMarshQMUL 0:a1caba5c4e48 88 while (true) {
WilliamMarshQMUL 0:a1caba5c4e48 89 float x, y, z;
WilliamMarshQMUL 0:a1caba5c4e48 90 x = acc.getAccX();
WilliamMarshQMUL 0:a1caba5c4e48 91 y = acc.getAccY();
WilliamMarshQMUL 0:a1caba5c4e48 92 z = acc.getAccZ();
duncangparker 4:efbc5085138a 93
duncangparker 5:75cd0f7649ca 94 state = orientation_find(x,y,z);
duncangparker 5:75cd0f7649ca 95
duncangparker 5:75cd0f7649ca 96 switch(system){
duncangparker 5:75cd0f7649ca 97 case 1: //initial flat state
duncangparker 5:75cd0f7649ca 98 if(tick > 100 && state == Yn){
duncangparker 5:75cd0f7649ca 99 pc.printf("passed first stage \n\r");
duncangparker 5:75cd0f7649ca 100 system = 2;
duncangparker 5:75cd0f7649ca 101 tick = 0;
duncangparker 5:75cd0f7649ca 102 }
duncangparker 5:75cd0f7649ca 103 else if( tick < 100 && state == Yn){
duncangparker 5:75cd0f7649ca 104 system = 6;
duncangparker 5:75cd0f7649ca 105 }
duncangparker 5:75cd0f7649ca 106 else if(state != Zp && state != IN){
duncangparker 5:75cd0f7649ca 107 system = 5;
duncangparker 4:efbc5085138a 108 }
duncangparker 4:efbc5085138a 109 break;
duncangparker 4:efbc5085138a 110
duncangparker 5:75cd0f7649ca 111 case 2: //right orientation state
duncangparker 5:75cd0f7649ca 112 if(tick > 20 && tick < 60 && state == Xn){
duncangparker 5:75cd0f7649ca 113 pc.printf("passed second stage \n\r");
duncangparker 5:75cd0f7649ca 114 system = 3;
duncangparker 5:75cd0f7649ca 115 tick = 0;
duncangparker 5:75cd0f7649ca 116 }
duncangparker 5:75cd0f7649ca 117 else if((tick > 20 && state == Xn) || tick > 60 ){
duncangparker 5:75cd0f7649ca 118 system = 6;
duncangparker 5:75cd0f7649ca 119 }
duncangparker 5:75cd0f7649ca 120 else if(state != Yn && state != IN){
duncangparker 5:75cd0f7649ca 121 system = 5;
duncangparker 5:75cd0f7649ca 122 }
duncangparker 5:75cd0f7649ca 123 break;
duncangparker 5:75cd0f7649ca 124
duncangparker 5:75cd0f7649ca 125 case 3: //upwards orientation
duncangparker 5:75cd0f7649ca 126 if(tick > 40 && tick < 80 && state == Zp){
duncangparker 5:75cd0f7649ca 127 pc.printf("passed third stage \n\r");
duncangparker 5:75cd0f7649ca 128 system = 4;
duncangparker 5:75cd0f7649ca 129 tick = 0;
duncangparker 5:75cd0f7649ca 130 }
duncangparker 5:75cd0f7649ca 131 else if((tick > 40 && state == Zp) || tick > 80 ){
duncangparker 5:75cd0f7649ca 132 system = 6;
duncangparker 5:75cd0f7649ca 133 }
duncangparker 5:75cd0f7649ca 134 else if(state != Xn && state != IN){
duncangparker 5:75cd0f7649ca 135 system = 5;
duncangparker 4:efbc5085138a 136 }
duncangparker 5:75cd0f7649ca 137 break;
duncangparker 5:75cd0f7649ca 138
duncangparker 5:75cd0f7649ca 139 case 4://flat success
duncangparker 5:75cd0f7649ca 140 pc.printf("Sequence is complete\n\r");
duncangparker 5:75cd0f7649ca 141 system = 7;
duncangparker 5:75cd0f7649ca 142 gled = 0;
duncangparker 5:75cd0f7649ca 143 break;
duncangparker 5:75cd0f7649ca 144
duncangparker 5:75cd0f7649ca 145 case 5://sequence fail
duncangparker 5:75cd0f7649ca 146 pc.printf("Sequence Error \n\r");
duncangparker 5:75cd0f7649ca 147 system = 7;
duncangparker 5:75cd0f7649ca 148 rled = 0;
duncangparker 5:75cd0f7649ca 149 break;
duncangparker 5:75cd0f7649ca 150
duncangparker 5:75cd0f7649ca 151 case 6://timing fail
duncangparker 5:75cd0f7649ca 152 pc.printf("Time error \n\r");
duncangparker 5:75cd0f7649ca 153 system = 7;
duncangparker 5:75cd0f7649ca 154 rled = 0;
duncangparker 5:75cd0f7649ca 155 break;
duncangparker 5:75cd0f7649ca 156
duncangparker 5:75cd0f7649ca 157 case 7://idle state
duncangparker 5:75cd0f7649ca 158 // nothing happens
duncangparker 5:75cd0f7649ca 159 break;
duncangparker 4:efbc5085138a 160
duncangparker 5:75cd0f7649ca 161 }// end switch
duncangparker 5:75cd0f7649ca 162 wait(0.1); // 100ms timing accuracy,
duncangparker 5:75cd0f7649ca 163 tick++;
duncangparker 4:efbc5085138a 164 }//end while
duncangparker 4:efbc5085138a 165 }//end main