File content as of revision 0:79959cf2cc3e:

#include "mbed.h"
#include "MPU6050.h"
#include "I2Cdev.h"
#include <CaseFsm.h>
#include <CaseEvents.h>
#include <Fsm.h>
#include <Events.h>
#include "MFRC522.h"

#include "log.h"
#include "DataLogger.h"

#define CURRENT_TIME 1510164886

#define SPI_MOSI    PA_7
#define SPI_MISO    PA_6
#define SPI_SCK     PA_5
#define SPI_CS      PA_8




int cbx[20];
int cby[20];
int cbz[20];



DigitalOut rfidpower(PA_12);

int x=0;

DigitalOut led1(LED1);
//Serial pc(USBTX,USBRX);
MPU6050 mpu;
bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD;
int8_t threshold;
bool zero_detect;
float temp;

//void flip(){test=1;}
Fsm* motionFsm;
CaseFsm* caseFsm;
SwitchPosition switchPosition;
RFIDEvent rfidEvent;
TimerEnd timerEnd;
Move move;
Stop stop;
uint8_t print=0;
//#include "PinDetect.h"

InterruptIn button1(PD_2);
InterruptIn button2(PC_11);
InterruptIn button3(PC_10);
InterruptIn button4(PC_12);//PRINT


InterruptIn mpuInterrupt(PB_5);

volatile uint8_t test = 0;
volatile uint8_t mot=0;

bool flag = false;
bool DeviceIsFull = false;

void button1PressedCallback()
{
    // printf("button1\n");
    caseFsm->handle(switchPosition);
}

void button2PressedCallback()
{
    //printf("button2\n");
    caseFsm->handle(rfidEvent);

}
void button3PressedCallback()
{
    //printf("button3\n");
    caseFsm->handle(timerEnd);

}

void button4PressedCallback()
{
    print=1;

}

void mpuInterruptCallback()
{
    test=1;

}

void getXYZMotions()
{
    wait(0.1);
    uint8_t motionByte = mpu.readThisByte(MPU6050_RA_MOT_DETECT_STATUS);
    // motionLog.motionByte = motionByte;
    //motionLog.timeStamp = time(NULL);
    //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    uint8_t b;
    bool zero = false;
    char x,y,z;
    int mx=0;
    int my=0;
    int mz=0;
    x=y=z='0';
    for(int i = 0; i<=7; i++) {
        b = motionByte & (1<<i);
        if(i==0 && b==1) {
            //time_t sec = time(NULL);
            //printf("Current time is: %s\r\n", ctime(&sec));
            zero = true;
            printf("ZERO MOTION detected.\r\n");
            //pc.printf("    %c    %c    %c     |    ",x,y,z);
            //pc.printf("%d;%d;%d;%d;%d;%d  ZERO DETECTED\r\n",ax,ay,az,gx,gy,gz);
        }
        if(i==1 && b==2) {
            printf("This should not happen :).\r\n");
        }
        if(i==6 && b==64) {
            x='+';
            mx=1;
            printf("X POSITIVE motion detected.\r\n");
        }
        if(i==7 && b==128) {
            x='-';
            mx=1;
            printf("X NEGATIVE motion detected.\r\n");
        }
        if(i==4 && b==16) {
            y='+';
            my=1;
            printf("Y POSITIVE motion detected.\r\n");
        }
        if(i==5 && b==32) {
            y='-';
            my=1;
            printf("Y NEGATIVE motion detected.\r\n");
        }
        if(i==2 && b==4) {
            z='+';
            mz=1;
            printf("Z POSITIVE motion detected.\r\n");
        }
        if(i==3 && b==8) {
            z='-';
            mz=1;
            printf("Z NEGATIVE motion detected.\r\n");
        }
    }
    printf("%d %d %d\r\n",mx,my,mz);


    int numX, numY, numZ;
    numX=numY=numZ=0;
    for(int i=0; i<=19; i++) {
        if(cbx[i]!=0) {
            numX++;
        }
        if(cby[i]!=0) {
            numY++;
        }
        if(cbz[i]!=0) {
            numZ++;
        }
    }
    //pc.printf("Motion sum: %d,%d,%d\r\n",numX,numY,numZ);
    if(numX<=10) {
        numX=0;
    } else
        numX=1;

    if(numY<=10) {
        numY=0;
    } else
        numY=1;

    if(numZ<=10) {
        numZ=0;
    } else
        numZ=1;

    /*
        //motionFsmLog.timeStamp = time(NULL);
        if(numX!=0 || numY!=0 || numZ!=0) {
            numX=1;
            numY=1;
            numZ=1;
            motionFsm->handle(move);
            if(mfl[mfl.get_size()-1].motionState != 1) {
                motionFsmLog.motionState = 1;
                mfl.push_back(motionFsmLog);
            }
        } else {
            motionFsm->handle(stop);
            if(mfl[mfl.get_size()-1].motionState != 0) {
                motionFsmLog.motionState = 0;
                mfl.push_back(motionFsmLog);
            }
        }

        //pc.printf("***************************\r\n");
        //pc.printf("%d %d %d\r\n",numX,numY,numZ);
        float fsmStateValues[3] = {numX,numY,numZ};


        */

}







MFRC522 RfChip(SPI_MOSI, SPI_MISO, SPI_SCK, SPI_CS, PB_4);

int main()
{

    rfidpower=1;

    Log mojlog;
    DataLogger <Log> mojlogger;
    mojlogger.print();
    mojlogger.savelog(mojlog);
    mojlogger.print();

    CaseFsm caseF;
    caseFsm = &caseF;

    //mpuInterrupt.mode(PullUp);
    button1.mode(PullUp);
    button2.mode(PullUp);
    button3.mode(PullUp);
    button4.mode(PullUp); //print
    button4.fall(button4PressedCallback);
    button3.fall(button3PressedCallback);
    button2.fall(button2PressedCallback);
    button1.fall(&button1PressedCallback);

    //mpuInterrupt.fall(mpuInterruptCallback);

    RfChip.PCD_Init();
    wait(1);






    ////////////////////MPU INTERUPT RADI//////////////////////////////////////

    mpuInterrupt.fall(mpuInterruptCallback);
    wait(1);
    mpu.calibrate(accelBias, gyroBias);
    mpu.initialize();
    mpu.setSleepEnabled(0);
    wait(1);
    mpu.setIntFreefallEnabled(1);
    mpu.setIntZeroMotionEnabled(1);
    mpu.setIntMotionEnabled(1);
    printf("Setting mpu parameters...\r\n");
    mpu.setMotionDetectionThreshold(1);
    mpu.setMotionDetectionDuration(15);
    mpu.setZeroMotionDetectionThreshold(1);
    mpu.setZeroMotionDetectionDuration(10);


    /////////////////////////////////////////////////////////////////////////////
    /*
     mpu.calibrate(accelBias, gyroBias);
    mpu.initialize();
    mpu.setSleepEnabled(0);
    mpu.setIntDMPEnabled(1);

    uint8_t pinValue = mpu.getIntEnabled();
    printf("Is int enabled? : %d\n", pinValue);
    printf("MPU6050 testing connection \n");


    wait(2);
    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult) {
        printf("MPU6050 test passed \n");
    } else {
        printf("MPU6050 test failed \n");
    }


    mpu.setIntEnabled(1);
    //mpu.setIntFreefallEnabled(1);
    //mpu.setIntZeroMotionEnabled(0);
    //mpu.setIntMotionEnabled(1);
    mpu.setDHPFMode(1);

    //*******************************************************************************************************************************

    printf("\n\n\n");
    uint8_t motionDetectionThreshold = mpu.getMotionDetectionThreshold();
    //printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);
    //printf("MPU6050 set motion detection threshold to 2\r\n");
    mpu.setMotionDetectionThreshold(1);
    motionDetectionThreshold = mpu.getMotionDetectionThreshold();
    //printf("MPU6050 motion detection threshold now is: %d \r\n", motionDetectionThreshold);
    printf("Motion detection threshold is: %d \r\n", motionDetectionThreshold);
    uint8_t motionDetectionDuration = mpu.getMotionDetectionDuration();
    //printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
    //printf("MPU6050 set motion detection duration to 40\r\n");
    mpu.setMotionDetectionDuration(20);
    motionDetectionDuration = mpu.getMotionDetectionDuration();
    //printf("MPU6050 motion detection duration now is: %d \r\n", motionDetectionDuration);
    printf("Motion detection duration is: %d \r\n", motionDetectionDuration);

    //*******************************************************************************************************************************

    //printf("\n\n\n");
    uint8_t zeroMotionDetectionThreshold = mpu.getZeroMotionDetectionThreshold();
    //printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
    //printf("MPU6050 set zero motion detection threshold to 2\r\n");
    mpu.setZeroMotionDetectionThreshold(2);
    zeroMotionDetectionThreshold = mpu.getZeroMotionDetectionThreshold();
    //printf("MPU6050 zero motion detection threshold now is: %d \r\n", zeroMotionDetectionThreshold);
    printf("Zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
    //printf("\n\n\n");
    int8_t zeroMotionDetectionDuration = mpu.getZeroMotionDetectionDuration();
    //printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
    //printf("MPU6050 set zero motion detection duration to 1\r\n");
    mpu.setZeroMotionDetectionDuration(1);
    zeroMotionDetectionDuration = mpu.getZeroMotionDetectionDuration();
    //printf("MPU6050 zero motion detection duration now is: %d \r\n", zeroMotionDetectionDuration);
    printf("Zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);

    //*******************************************************************************************************************************

    //printf("\n\n\n");
    int8_t freefallDetectionThreshold = mpu.getFreefallDetectionThreshold();
    //printf("MPU6050 freefall motion detection threshold is: %d \r\n", freefallDetectionThreshold);
    //printf("MPU6050 set freefall motion detection threshold to 40\r\n");
    //mpu.setFreefallDetectionThreshold(2);
    freefallDetectionThreshold = mpu.getFreefallDetectionThreshold();
    //printf("MPU6050 freefall motion detection threshold now is: %d \r\n", freefallDetectionThreshold);
    printf("Freefall motion detection threshold is: %d \r\n", freefallDetectionThreshold);
    //printf("\n\n\n");
    int8_t freefallDetectionDuration = mpu.getFreefallDetectionDuration();
    //printf("MPU6050 freefall motion detection duration is: %d \r\n", freefallDetectionDuration);
    //printf("MPU6050 set freefall motion detection duration to 40\r\n");
    //mpu.setFreefallDetectionDuration(40);
    freefallDetectionDuration = mpu.getFreefallDetectionDuration();
    //printf("MPU6050 freefall motion detection duration now is: %d \r\n", freefallDetectionDuration);
    printf("Freefall motion detection duration is: %d \r\n", freefallDetectionDuration);
    printf("\n\n\n");
    printf("MPU6050 test\n\n");


    //*******************************************************************************************************************************
    */



    while(1) {

        if(test==1) {
            //wait(1);
            //int ax,az,az,gz,gy,gz;
            mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            printf("%d;%d;%d;%d;%d;%d  ZERO DETECTED\r\n",ax,ay,az,gx,gy,gz);
            printf("nooooo\n");
            test = 1;
            mot=2;
        }
        if(test==0) {
            //wait(1);
            printf("yes\n");
            test=0;
            mot=2;
        }


        if(print==1) {
            mojlogger.print();
            print=0;
        }



//printf("caseFsm->DeviceInFullState()=%d\n",caseFsm->DeviceInFullState());
        //This goes into caseFsm, but for now we are testing in main.cpp
        if(caseFsm->DeviceInFullState()) {
            printf("Waiting 5 seconds for RFID event...\r\n");
            while (caseFsm->DeviceInFullState()) {
                // Look for new cards


                if ( ! RfChip.PICC_IsNewCardPresent()) {
                    wait_ms(500);
                    printf("!RfChip.PICC_IsNewCardPresent()\n");
                    continue;
                }

                // Select one of the cards
                if ( ! RfChip.PICC_ReadCardSerial()) {
                    printf("!RfChip.PICC_ReadCardSerial()\n");
                    wait_ms(500);
                    continue;
                }

                // Print Card UID
                printf("Card UID: ");
                for (uint8_t i = 0; i < RfChip.uid.size; i++) {
                    rfidEvent.uid[i] = RfChip.uid.uidByte[i];
                    printf("%X ", RfChip.uid.uidByte[i]);
                }
                rfidEvent.size = RfChip.uid.size;
                printf("\n\r");

                // Print Card type
                uint8_t piccType = RfChip.PICC_GetType(RfChip.uid.sak);
                printf("PICC Type: %s \n\r", RfChip.PICC_GetTypeName(piccType));
                wait_ms(1000);
                rfidEvent.cardType = piccType;
                //phone inserted
                caseFsm->handle(rfidEvent);
                printf("_%d_\n",caseFsm->DeviceInPhoneDetectedState());
                if(caseFsm->DeviceInPhoneDetectedState()) {
                    wait(0.3);
                    x++;
                    printf("snimi\n");
                    mojlog.setLog(1100+x,1,0,1300);
                    mojlogger.savelog(mojlog);
                    ;//Loguj phone in i vrijeme i datum

                }

            }
            if(caseFsm->DeviceInDeviceEmptyState()) {
                wait(0.3);
                printf("ne snimi\n");
                x++;   //Loguj phone out i vrijeme i datum
                mojlog.setLog(1100+x,0,0,1400);
                mojlogger.savelog(mojlog);
            }
        }



    }

}