Edwin Kadavy / Mbed OS Step-3_lab-4_edwin_kadavy

Step-3_lab-4_edwin_kadavy

Dependencies:   MMA8451Q

Fork of Accelerometer_lab_4_edwin_kadavy by Edwin Kadavy

main.cpp@3:c8013ce381bc, 2018-02-22 (annotated)

Committer:
edwinkad
Date:
Thu Feb 22 15:02:47 2018 +0000
Revision:
3:c8013ce381bc
Parent:
2:b884d78f375d 
new;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WilliamMarshQMUL 0:a1caba5c4e48 1 #include "mbed.h"
WilliamMarshQMUL 0:a1caba5c4e48 2 #include "rtos.h"
WilliamMarshQMUL 0:a1caba5c4e48 3 #include "MMA8451Q.h"
edwinkad 3:c8013ce381bc 4 #define MMA8451_I2C_ADDRESS (0x1d<<1)
WilliamMarshQMUL 0:a1caba5c4e48 5
edwinkad 2:b884d78f375d 6 PinName const SDA = PTE25;
edwinkad 2:b884d78f375d 7 PinName const SCL = PTE24;
edwinkad 3:c8013ce381bc 8 Thread threadLed, threadAcc;
edwinkad 3:c8013ce381bc 9 Timer t1, t2;
edwinkad 3:c8013ce381bc 10 float timer1, timer2, timer3, totalTimer;
edwinkad 3:c8013ce381bc 11 int seqError;
edwinkad 3:c8013ce381bc 12 int timePrint, statePrint, printReset, printFlat;
edwinkad 3:c8013ce381bc 13 volatile int states;
edwinkad 3:c8013ce381bc 14 volatile int accState;
WilliamMarshQMUL 0:a1caba5c4e48 15
WilliamMarshQMUL 0:a1caba5c4e48 16
edwinkad 3:c8013ce381bc 17 PwmOut rled(LED1);
edwinkad 3:c8013ce381bc 18 PwmOut gled(LED2);
edwinkad 3:c8013ce381bc 19 PwmOut bled(LED3);
edwinkad 3:c8013ce381bc 20 Serial pc(USBTX, USBRX); // tx, rx
edwinkad 3:c8013ce381bc 21 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
edwinkad 3:c8013ce381bc 22
edwinkad 3:c8013ce381bc 23
edwinkad 3:c8013ce381bc 24 void stateAcc()
WilliamMarshQMUL 0:a1caba5c4e48 25 {
edwinkad 3:c8013ce381bc 26 t1.start();
edwinkad 3:c8013ce381bc 27 t2.start();
edwinkad 3:c8013ce381bc 28 timePrint=1;
edwinkad 3:c8013ce381bc 29 statePrint=1;
edwinkad 3:c8013ce381bc 30 float resetTime=10;
edwinkad 3:c8013ce381bc 31 int prAccState=0;
edwinkad 3:c8013ce381bc 32 float x, y, z;
edwinkad 3:c8013ce381bc 33 float accmin=0.95;
edwinkad 3:c8013ce381bc 34 float accmax=1.05;
edwinkad 3:c8013ce381bc 35 float acc2min=0.90;
edwinkad 3:c8013ce381bc 36 float acc2max=1.10;
edwinkad 3:c8013ce381bc 37 int accloop=1;
edwinkad 3:c8013ce381bc 38 while (true) {
WilliamMarshQMUL 0:a1caba5c4e48 39
WilliamMarshQMUL 0:a1caba5c4e48 40 x = acc.getAccX();
WilliamMarshQMUL 0:a1caba5c4e48 41 y = acc.getAccY();
WilliamMarshQMUL 0:a1caba5c4e48 42 z = acc.getAccZ();
edwinkad 3:c8013ce381bc 43 if (accloop==1) {
edwinkad 3:c8013ce381bc 44 if(z>=accmin && z<=accmax) {
edwinkad 3:c8013ce381bc 45 accState=1;
edwinkad 3:c8013ce381bc 46 } else if(y<=-(accmin) && y>=-(accmax)) {
edwinkad 3:c8013ce381bc 47 accState=2;
edwinkad 3:c8013ce381bc 48 } else if(y>=accmin && y<=accmax) {
edwinkad 3:c8013ce381bc 49 accState=3;
edwinkad 3:c8013ce381bc 50 } else if(x<=-(accmin) && x>=-(accmax)) {
edwinkad 3:c8013ce381bc 51 accState=4;
edwinkad 3:c8013ce381bc 52 } else if(x>=accmin && x<=accmax) {
edwinkad 3:c8013ce381bc 53 accState=5;
edwinkad 3:c8013ce381bc 54 } else if(z<=-(accmin) && z>=-(accmax)) {
edwinkad 3:c8013ce381bc 55 accState=6;
edwinkad 3:c8013ce381bc 56 } else {
edwinkad 3:c8013ce381bc 57 accState=0;
edwinkad 3:c8013ce381bc 58 }
edwinkad 3:c8013ce381bc 59 }
edwinkad 3:c8013ce381bc 60
edwinkad 3:c8013ce381bc 61 if(accState==1) {
edwinkad 3:c8013ce381bc 62 if(z>=acc2min && z<=acc2max)accloop=0;
edwinkad 3:c8013ce381bc 63 else accloop=1;
edwinkad 3:c8013ce381bc 64 } else if(accState==2) {
edwinkad 3:c8013ce381bc 65 if(y<=-(acc2min) && y>=-(acc2max))accloop=0;
edwinkad 3:c8013ce381bc 66 else accloop=1;
edwinkad 3:c8013ce381bc 67 } else if(accState==3) {
edwinkad 3:c8013ce381bc 68 if(y>=acc2min && y<=acc2max)accloop=0;
edwinkad 3:c8013ce381bc 69 else accloop=1;
edwinkad 3:c8013ce381bc 70 } else if(accState==4) {
edwinkad 3:c8013ce381bc 71 if(x<=-(acc2min) && x>=-(acc2max))accloop=0;
edwinkad 3:c8013ce381bc 72 else accloop=1;
edwinkad 3:c8013ce381bc 73 } else if(accState==5) {
edwinkad 3:c8013ce381bc 74 if(x>=acc2min && x<=acc2max)accloop=0;
edwinkad 3:c8013ce381bc 75 else accloop=1;
edwinkad 3:c8013ce381bc 76 } else if(accState==6) {
edwinkad 3:c8013ce381bc 77 if(z<=-(acc2min) && z>=-(acc2max))accloop=0;
edwinkad 3:c8013ce381bc 78 else accloop=1;
edwinkad 3:c8013ce381bc 79 }
edwinkad 3:c8013ce381bc 80
edwinkad 3:c8013ce381bc 81 if(accState!=prAccState) {
edwinkad 3:c8013ce381bc 82 switch (accState) {
edwinkad 3:c8013ce381bc 83 case 0: //Intermidiate
edwinkad 3:c8013ce381bc 84 pc.printf("positioning\n\r");
edwinkad 3:c8013ce381bc 85 break;
edwinkad 3:c8013ce381bc 86 case 1: //Flat
edwinkad 3:c8013ce381bc 87 pc.printf("flat\n\r");
edwinkad 3:c8013ce381bc 88 break;
edwinkad 3:c8013ce381bc 89 case 2: //left
edwinkad 3:c8013ce381bc 90 pc.printf("left\n\r");
edwinkad 3:c8013ce381bc 91 break;
edwinkad 3:c8013ce381bc 92 case 3: //right
edwinkad 3:c8013ce381bc 93 pc.printf("right\n\r");
edwinkad 3:c8013ce381bc 94 break;
edwinkad 3:c8013ce381bc 95 case 4: //down
edwinkad 3:c8013ce381bc 96 pc.printf("down\n\r");
edwinkad 3:c8013ce381bc 97 break;
edwinkad 3:c8013ce381bc 98 case 5: //up
edwinkad 3:c8013ce381bc 99 pc.printf("up\n\r");
edwinkad 3:c8013ce381bc 100 break;
edwinkad 3:c8013ce381bc 101 case 6: //over
edwinkad 3:c8013ce381bc 102 pc.printf("over\n\r");
edwinkad 3:c8013ce381bc 103 break;
edwinkad 3:c8013ce381bc 104 }
edwinkad 3:c8013ce381bc 105 }
edwinkad 3:c8013ce381bc 106 prAccState=accState;
edwinkad 3:c8013ce381bc 107
edwinkad 3:c8013ce381bc 108 if(timePrint==1 && accState!=0)pc.printf("%1.2f s\n\r",t1.read());
edwinkad 3:c8013ce381bc 109
edwinkad 3:c8013ce381bc 110 if (states==0) { //normal state
edwinkad 3:c8013ce381bc 111 rled=1;
edwinkad 3:c8013ce381bc 112 gled=1;
edwinkad 3:c8013ce381bc 113 bled=!bled;
edwinkad 3:c8013ce381bc 114 //do nothing
edwinkad 3:c8013ce381bc 115 } else if(states==1) { //trigger for green Led
edwinkad 3:c8013ce381bc 116 rled=1;
edwinkad 3:c8013ce381bc 117 gled=0;
edwinkad 3:c8013ce381bc 118 bled=1;
edwinkad 3:c8013ce381bc 119 } else if(states==2) { //state sequence error
edwinkad 3:c8013ce381bc 120 if(statePrint==1) {
edwinkad 3:c8013ce381bc 121 pc.printf("State sequence error!!! please return to flat postion\n\r");
edwinkad 3:c8013ce381bc 122 statePrint=0;
edwinkad 3:c8013ce381bc 123 }
edwinkad 3:c8013ce381bc 124 rled=0;
edwinkad 3:c8013ce381bc 125 gled=1;
edwinkad 3:c8013ce381bc 126 bled=1;
edwinkad 3:c8013ce381bc 127 } else if(states==3) { //state time error
edwinkad 3:c8013ce381bc 128 if(statePrint==1) {
edwinkad 3:c8013ce381bc 129 pc.printf("State time error!!! please return to flat postion\n\r");
edwinkad 3:c8013ce381bc 130 statePrint=0;
edwinkad 3:c8013ce381bc 131 }
edwinkad 3:c8013ce381bc 132 rled=0;
edwinkad 3:c8013ce381bc 133 gled=1;
edwinkad 3:c8013ce381bc 134 bled=1;
edwinkad 3:c8013ce381bc 135 } else if(states==4) { //reset state
edwinkad 3:c8013ce381bc 136 pc.printf("sequence reseting-> wait %1.2f s\n\r",(resetTime-t1.read()));
edwinkad 3:c8013ce381bc 137 rled=!rled;
edwinkad 3:c8013ce381bc 138 gled=1;
edwinkad 3:c8013ce381bc 139 bled=1;
edwinkad 3:c8013ce381bc 140 }
edwinkad 3:c8013ce381bc 141 Thread::wait(300);//check where this should be
edwinkad 3:c8013ce381bc 142 }
edwinkad 3:c8013ce381bc 143 }
edwinkad 2:b884d78f375d 144
edwinkad 2:b884d78f375d 145
edwinkad 3:c8013ce381bc 146 int main(void)
edwinkad 3:c8013ce381bc 147 {
edwinkad 3:c8013ce381bc 148 int seq=1;
edwinkad 3:c8013ce381bc 149 int endprog=0;
edwinkad 3:c8013ce381bc 150
edwinkad 3:c8013ce381bc 151 //pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI());
edwinkad 3:c8013ce381bc 152
edwinkad 3:c8013ce381bc 153 rled=1;
edwinkad 3:c8013ce381bc 154 gled=1;
edwinkad 3:c8013ce381bc 155 bled=1;
edwinkad 3:c8013ce381bc 156 //threadLed.start(callback(stateLed));
edwinkad 3:c8013ce381bc 157 threadAcc.start(callback(stateAcc));
edwinkad 3:c8013ce381bc 158 Thread::wait(100);
edwinkad 3:c8013ce381bc 159 /*orient->orentation states
edwinkad 3:c8013ce381bc 160 orient=1
edwinkad 3:c8013ce381bc 161 */
edwinkad 3:c8013ce381bc 162 //flat position
edwinkad 3:c8013ce381bc 163 while (true) {
edwinkad 3:c8013ce381bc 164 if (seq==0) {
edwinkad 3:c8013ce381bc 165 timePrint=0;
edwinkad 3:c8013ce381bc 166 //statePrint=0;
edwinkad 3:c8013ce381bc 167 if(accState==1) {
edwinkad 3:c8013ce381bc 168 states=4;
edwinkad 3:c8013ce381bc 169 statePrint=1;
edwinkad 3:c8013ce381bc 170 t1.reset();
edwinkad 3:c8013ce381bc 171 while(accState==1) {
edwinkad 3:c8013ce381bc 172 if(t1.read()>=10) {
edwinkad 3:c8013ce381bc 173 t1.reset();
edwinkad 3:c8013ce381bc 174 timePrint=1;
edwinkad 3:c8013ce381bc 175 //statePrint=1;
edwinkad 3:c8013ce381bc 176 seq=1;
edwinkad 3:c8013ce381bc 177 t2.reset();
edwinkad 3:c8013ce381bc 178 break;
edwinkad 3:c8013ce381bc 179 }
edwinkad 3:c8013ce381bc 180 //printReset=1;
edwinkad 3:c8013ce381bc 181 }
edwinkad 3:c8013ce381bc 182 } else {
edwinkad 3:c8013ce381bc 183 states=3;
edwinkad 3:c8013ce381bc 184 }
edwinkad 3:c8013ce381bc 185 }
edwinkad 3:c8013ce381bc 186 if(seq==1)pc.printf("Starting Sequence\n\r");
edwinkad 3:c8013ce381bc 187 while(seq==1) {
edwinkad 3:c8013ce381bc 188 if (accState==1) {
edwinkad 3:c8013ce381bc 189 states=0;
edwinkad 3:c8013ce381bc 190 if(t1.read()<10) {
edwinkad 3:c8013ce381bc 191 if(accState!=1) {
edwinkad 3:c8013ce381bc 192 states=3;
edwinkad 3:c8013ce381bc 193 seq=0;
edwinkad 3:c8013ce381bc 194 }
edwinkad 3:c8013ce381bc 195 } else if (t1.read()>=10) {
edwinkad 3:c8013ce381bc 196 states=0;
edwinkad 3:c8013ce381bc 197 if(accState!=1)seq=2;
edwinkad 3:c8013ce381bc 198 }
edwinkad 3:c8013ce381bc 199 } else if (accState!=1) {
edwinkad 3:c8013ce381bc 200 states=2;
edwinkad 3:c8013ce381bc 201 seq=0;
edwinkad 3:c8013ce381bc 202 }
edwinkad 3:c8013ce381bc 203 }
edwinkad 3:c8013ce381bc 204 timer1=t1.read();
edwinkad 3:c8013ce381bc 205 //positioning
edwinkad 3:c8013ce381bc 206 while(seq==2) {
edwinkad 3:c8013ce381bc 207 if(accState>0) {
edwinkad 3:c8013ce381bc 208 seq=3;
edwinkad 3:c8013ce381bc 209 }
edwinkad 2:b884d78f375d 210 }
edwinkad 3:c8013ce381bc 211 //right position
edwinkad 3:c8013ce381bc 212 t1.reset();
edwinkad 3:c8013ce381bc 213 while(seq==3) {
edwinkad 3:c8013ce381bc 214 if(accState==3) {
edwinkad 3:c8013ce381bc 215 states=0;
edwinkad 3:c8013ce381bc 216 if(t1.read()<2) {
edwinkad 3:c8013ce381bc 217 if(accState!=3) {
edwinkad 3:c8013ce381bc 218 states=3;
edwinkad 3:c8013ce381bc 219 seq=0;
edwinkad 3:c8013ce381bc 220 }
edwinkad 3:c8013ce381bc 221 } else if (t1.read()>=2 && t1.read()<=6) {
edwinkad 3:c8013ce381bc 222 states=0;
edwinkad 3:c8013ce381bc 223 if(accState!=3)seq=4;
edwinkad 3:c8013ce381bc 224 } else if (t1.read()>6) {
edwinkad 3:c8013ce381bc 225 states=3;
edwinkad 3:c8013ce381bc 226 seq=0;
edwinkad 3:c8013ce381bc 227 }
edwinkad 3:c8013ce381bc 228 } else if(accState!=3) {
edwinkad 3:c8013ce381bc 229 states=2;
edwinkad 3:c8013ce381bc 230 seq=0;
edwinkad 3:c8013ce381bc 231 }
edwinkad 3:c8013ce381bc 232 }
edwinkad 3:c8013ce381bc 233 timer2=t1.read();
edwinkad 3:c8013ce381bc 234 //positioning
edwinkad 3:c8013ce381bc 235 while(seq==4) {
edwinkad 3:c8013ce381bc 236 if(accState>0) {
edwinkad 3:c8013ce381bc 237 seq=5;
edwinkad 3:c8013ce381bc 238 }
edwinkad 3:c8013ce381bc 239 }
edwinkad 3:c8013ce381bc 240 //up position
edwinkad 3:c8013ce381bc 241 t1.reset();
edwinkad 3:c8013ce381bc 242 while(seq==5) {
edwinkad 3:c8013ce381bc 243 if(accState==5) {
edwinkad 3:c8013ce381bc 244 states=0;
edwinkad 3:c8013ce381bc 245 if(t1.read()<4) {
edwinkad 3:c8013ce381bc 246 if(accState!=5) {
edwinkad 3:c8013ce381bc 247 states=3;
edwinkad 3:c8013ce381bc 248 seq=0;
edwinkad 3:c8013ce381bc 249 }
edwinkad 3:c8013ce381bc 250 } else if (t1.read()>=4 && t1.read()<=8) {
edwinkad 3:c8013ce381bc 251 states=0;
edwinkad 3:c8013ce381bc 252 if(accState!=5)seq=6;
edwinkad 3:c8013ce381bc 253 } else if (t1.read()>8) {
edwinkad 3:c8013ce381bc 254 states=3;
edwinkad 3:c8013ce381bc 255 seq=0;
edwinkad 3:c8013ce381bc 256 }
edwinkad 3:c8013ce381bc 257 } else if(accState!=3) {
edwinkad 3:c8013ce381bc 258 states=2;
edwinkad 3:c8013ce381bc 259 seq=0;
edwinkad 3:c8013ce381bc 260 }
edwinkad 3:c8013ce381bc 261 }
edwinkad 3:c8013ce381bc 262 timer3=t1.read();
edwinkad 3:c8013ce381bc 263 //positioning
edwinkad 3:c8013ce381bc 264 while(seq==6) {
edwinkad 3:c8013ce381bc 265 if(accState>0) {
edwinkad 3:c8013ce381bc 266 seq=7;
edwinkad 3:c8013ce381bc 267 }
edwinkad 3:c8013ce381bc 268 }
edwinkad 3:c8013ce381bc 269 timePrint=0;
edwinkad 3:c8013ce381bc 270
edwinkad 3:c8013ce381bc 271 totalTimer=t2.read();
edwinkad 3:c8013ce381bc 272 while(seq==7) {
edwinkad 3:c8013ce381bc 273 if(accState==1) {
edwinkad 3:c8013ce381bc 274 states=1;
edwinkad 3:c8013ce381bc 275 t1.stop();
edwinkad 3:c8013ce381bc 276 t2.stop();
edwinkad 3:c8013ce381bc 277 endprog=1;
edwinkad 2:b884d78f375d 278 break;
edwinkad 3:c8013ce381bc 279 }
edwinkad 2:b884d78f375d 280 }
edwinkad 3:c8013ce381bc 281 if (seq==7 && endprog==1)break;
WilliamMarshQMUL 0:a1caba5c4e48 282 }
edwinkad 3:c8013ce381bc 283 pc.printf("triggered --- Sequence complete\n\r");
edwinkad 3:c8013ce381bc 284 pc.printf("flat position:%1.2f s\n\rright position:%1.2f s\n\rup position:%1.2f s\n\rtotal time:%1.2f s\n\r",timer1,timer2,timer3,totalTimer);
edwinkad 3:c8013ce381bc 285 }