Step-3_lab-4_edwin_kadavy
Fork of Accelerometer_lab_4_edwin_kadavy by
main.cpp
- Committer:
- edwinkad
- Date:
- 2018-02-22
- Revision:
- 3:c8013ce381bc
- Parent:
- 2:b884d78f375d
File content as of revision 3:c8013ce381bc:
#include "mbed.h" #include "rtos.h" #include "MMA8451Q.h" #define MMA8451_I2C_ADDRESS (0x1d<<1) PinName const SDA = PTE25; PinName const SCL = PTE24; Thread threadLed, threadAcc; Timer t1, t2; float timer1, timer2, timer3, totalTimer; int seqError; int timePrint, statePrint, printReset, printFlat; volatile int states; volatile int accState; PwmOut rled(LED1); PwmOut gled(LED2); PwmOut bled(LED3); Serial pc(USBTX, USBRX); // tx, rx MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); void stateAcc() { t1.start(); t2.start(); timePrint=1; statePrint=1; float resetTime=10; int prAccState=0; float x, y, z; float accmin=0.95; float accmax=1.05; float acc2min=0.90; float acc2max=1.10; int accloop=1; while (true) { x = acc.getAccX(); y = acc.getAccY(); z = acc.getAccZ(); if (accloop==1) { if(z>=accmin && z<=accmax) { accState=1; } else if(y<=-(accmin) && y>=-(accmax)) { accState=2; } else if(y>=accmin && y<=accmax) { accState=3; } else if(x<=-(accmin) && x>=-(accmax)) { accState=4; } else if(x>=accmin && x<=accmax) { accState=5; } else if(z<=-(accmin) && z>=-(accmax)) { accState=6; } else { accState=0; } } if(accState==1) { if(z>=acc2min && z<=acc2max)accloop=0; else accloop=1; } else if(accState==2) { if(y<=-(acc2min) && y>=-(acc2max))accloop=0; else accloop=1; } else if(accState==3) { if(y>=acc2min && y<=acc2max)accloop=0; else accloop=1; } else if(accState==4) { if(x<=-(acc2min) && x>=-(acc2max))accloop=0; else accloop=1; } else if(accState==5) { if(x>=acc2min && x<=acc2max)accloop=0; else accloop=1; } else if(accState==6) { if(z<=-(acc2min) && z>=-(acc2max))accloop=0; else accloop=1; } if(accState!=prAccState) { switch (accState) { case 0: //Intermidiate pc.printf("positioning\n\r"); break; case 1: //Flat pc.printf("flat\n\r"); break; case 2: //left pc.printf("left\n\r"); break; case 3: //right pc.printf("right\n\r"); break; case 4: //down pc.printf("down\n\r"); break; case 5: //up pc.printf("up\n\r"); break; case 6: //over pc.printf("over\n\r"); break; } } prAccState=accState; if(timePrint==1 && accState!=0)pc.printf("%1.2f s\n\r",t1.read()); if (states==0) { //normal state rled=1; gled=1; bled=!bled; //do nothing } else if(states==1) { //trigger for green Led rled=1; gled=0; bled=1; } else if(states==2) { //state sequence error if(statePrint==1) { pc.printf("State sequence error!!! please return to flat postion\n\r"); statePrint=0; } rled=0; gled=1; bled=1; } else if(states==3) { //state time error if(statePrint==1) { pc.printf("State time error!!! please return to flat postion\n\r"); statePrint=0; } rled=0; gled=1; bled=1; } else if(states==4) { //reset state pc.printf("sequence reseting-> wait %1.2f s\n\r",(resetTime-t1.read())); rled=!rled; gled=1; bled=1; } Thread::wait(300);//check where this should be } } int main(void) { int seq=1; int endprog=0; //pc.printf("MMA8451 ID: %d\n", acc.getWhoAmI()); rled=1; gled=1; bled=1; //threadLed.start(callback(stateLed)); threadAcc.start(callback(stateAcc)); Thread::wait(100); /*orient->orentation states orient=1 */ //flat position while (true) { if (seq==0) { timePrint=0; //statePrint=0; if(accState==1) { states=4; statePrint=1; t1.reset(); while(accState==1) { if(t1.read()>=10) { t1.reset(); timePrint=1; //statePrint=1; seq=1; t2.reset(); break; } //printReset=1; } } else { states=3; } } if(seq==1)pc.printf("Starting Sequence\n\r"); while(seq==1) { if (accState==1) { states=0; if(t1.read()<10) { if(accState!=1) { states=3; seq=0; } } else if (t1.read()>=10) { states=0; if(accState!=1)seq=2; } } else if (accState!=1) { states=2; seq=0; } } timer1=t1.read(); //positioning while(seq==2) { if(accState>0) { seq=3; } } //right position t1.reset(); while(seq==3) { if(accState==3) { states=0; if(t1.read()<2) { if(accState!=3) { states=3; seq=0; } } else if (t1.read()>=2 && t1.read()<=6) { states=0; if(accState!=3)seq=4; } else if (t1.read()>6) { states=3; seq=0; } } else if(accState!=3) { states=2; seq=0; } } timer2=t1.read(); //positioning while(seq==4) { if(accState>0) { seq=5; } } //up position t1.reset(); while(seq==5) { if(accState==5) { states=0; if(t1.read()<4) { if(accState!=5) { states=3; seq=0; } } else if (t1.read()>=4 && t1.read()<=8) { states=0; if(accState!=5)seq=6; } else if (t1.read()>8) { states=3; seq=0; } } else if(accState!=3) { states=2; seq=0; } } timer3=t1.read(); //positioning while(seq==6) { if(accState>0) { seq=7; } } timePrint=0; totalTimer=t2.read(); while(seq==7) { if(accState==1) { states=1; t1.stop(); t2.stop(); endprog=1; break; } } if (seq==7 && endprog==1)break; } pc.printf("triggered --- Sequence complete\n\r"); 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); }