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);
}