main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #include "I2Cdev.h"
00004 #include <CaseFsm.h>
00005 #include <CaseEvents.h>
00006 #include <Fsm.h>
00007 #include <Events.h>
00008 #include "MFRC522.h"
00009 
00010 #include "log.h"
00011 #include "DataLogger.h"
00012 
00013 #define CURRENT_TIME 1510164886
00014 
00015 #define SPI_MOSI    PA_7
00016 #define SPI_MISO    PA_6
00017 #define SPI_SCK     PA_5
00018 #define SPI_CS      PA_8
00019 
00020 
00021 
00022 
00023 int cbx[20];
00024 int cby[20];
00025 int cbz[20];
00026 
00027 
00028     Log mojlog;
00029     DataLogger <Log> mojlogger;
00030   //  mojlogger.print();
00031    // mojlogger.savelog(mojlog);
00032    // mojlogger.print();
00033 
00034 
00035 
00036 DigitalOut rfidpower(PA_12);
00037 
00038 int x=0;
00039 
00040 DigitalOut led1(LED1);
00041 //Serial pc(USBTX,USBRX);
00042 
00043 bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD;
00044 int8_t threshold;
00045 bool zero_detect;
00046 float temp;
00047 
00048 //void flip(){test=1;}
00049 Fsm* motionFsm;
00050 CaseFsm* caseFsm;
00051 SwitchPosition switchPosition;
00052 RFIDEvent rfidEvent;
00053 TimerEnd timerEnd;
00054 Move move;
00055 Stop stop;
00056 uint8_t print=0;
00057 //#include "PinDetect.h"
00058 
00059 InterruptIn button1(PD_2);
00060 InterruptIn button2(PC_11);
00061 InterruptIn button3(PC_10);
00062 InterruptIn button4(PC_12);//PRINT
00063 
00064 
00065 InterruptIn mpuInterrupt(PB_5);
00066 
00067 volatile uint8_t test = 0;
00068 volatile uint8_t mot=0;
00069 
00070 bool flag = false;
00071 bool DeviceIsFull = false;
00072 
00073 void button1PressedCallback()
00074 {
00075     // printf("button1\n");
00076     caseFsm->handle(switchPosition);
00077 }
00078 
00079 void button2PressedCallback()
00080 {
00081     //printf("button2\n");
00082     caseFsm->handle(rfidEvent);
00083 
00084 }
00085 void button3PressedCallback()
00086 {
00087     //printf("button3\n");
00088     caseFsm->handle(timerEnd);
00089 
00090 }
00091 
00092 void button4PressedCallback()
00093 {
00094     print=1;
00095 
00096 }
00097 
00098 void mpuInterruptCallback()
00099 {
00100     test=1;
00101 }
00102 
00103 
00104 MPU6050 mpu;
00105 uint8_t nomotion=0;
00106 uint8_t motionlogged=0;
00107 
00108 void getXYZMotions()
00109 {
00110     wait(0.1);
00111     uint8_t motionByte = mpu.readThisByte(MPU6050_RA_MOT_DETECT_STATUS);
00112     // motionLog.motionByte = motionByte;
00113     //motionLog.timeStamp = time(NULL);
00114     //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00115     uint8_t b;
00116     bool zero = false;
00117     char x,y,z;
00118     int mx=0;
00119     int my=0;
00120     int mz=0;
00121     x=y=z='0';
00122     for(int i = 0; i<=7; i++) {
00123         b = motionByte & (1<<i);
00124         if(i==0 && b==1) {
00125             //time_t sec = time(NULL);
00126             //printf("Current time is: %s\r\n", ctime(&sec));
00127             zero = true;
00128             printf("ZERO MOTION detected.\r\n");
00129             //pc.printf("    %c    %c    %c     |    ",x,y,z);
00130             //pc.printf("%d;%d;%d;%d;%d;%d  ZERO DETECTED\r\n",ax,ay,az,gx,gy,gz);
00131         }
00132         if(i==1 && b==2) {
00133             printf("This should not happen :).\r\n");
00134         }
00135         if(i==6 && b==64) {
00136             x='+';
00137             mx=1;
00138             printf("X POSITIVE motion detected.\r\n");
00139         }
00140         if(i==7 && b==128) {
00141             x='-';
00142             mx=1;
00143             printf("X NEGATIVE motion detected.\r\n");
00144         }
00145         if(i==4 && b==16) {
00146             y='+';
00147             my=1;
00148             printf("Y POSITIVE motion detected.\r\n");
00149         }
00150         if(i==5 && b==32) {
00151             y='-';
00152             my=1;
00153             printf("Y NEGATIVE motion detected.\r\n");
00154         }
00155         if(i==2 && b==4) {
00156             z='+';
00157             mz=1;
00158             printf("Z POSITIVE motion detected.\r\n");
00159         }
00160         if(i==3 && b==8) {
00161             z='-';
00162             mz=1;
00163             printf("Z NEGATIVE motion detected.\r\n");
00164         }
00165     }
00166     printf("%d %d %d\r\n",mx,my,mz);
00167 
00168     if(mx==0 && my==0 && mz==0) {
00169         nomotion++;
00170         if(nomotion==3 && motionlogged==0) {
00171             printf("SAVE NO motion\n");
00172             //motionFsm->handle(stop);
00173             mojlog.setLog(900,0,0,900);
00174             mojlogger.savelog(mojlog);
00175             motionlogged=1;
00176             nomotion=0;
00177         }
00178         }
00179 
00180         else if(motionlogged==1) {
00181             printf("SAVE motion\n");
00182             motionlogged=0;
00183             //motionFsm->handle(move);
00184             mojlog.setLog(2222,0,1,2222);
00185             mojlogger.savelog(mojlog);
00186             nomotion=0;
00187         }
00188 
00189 
00190 
00191 
00192 
00193 
00194 
00195 
00196 }
00197 
00198 
00199 
00200 
00201 
00202 
00203 
00204 MFRC522 RfChip(SPI_MOSI, SPI_MISO, SPI_SCK, SPI_CS, PB_4);
00205 
00206 int main()
00207 {
00208 
00209     rfidpower=1;
00210 
00211 
00212 
00213     CaseFsm caseF;
00214     caseFsm = &caseF;
00215 
00216     //mpuInterrupt.mode(PullUp);
00217     button1.mode(PullUp);
00218     button2.mode(PullUp);
00219     button3.mode(PullUp);
00220     button4.mode(PullUp); //print
00221     button4.fall(button4PressedCallback);
00222     button3.fall(button3PressedCallback);
00223     button2.fall(button2PressedCallback);
00224     button1.fall(&button1PressedCallback);
00225 
00226     //mpuInterrupt.fall(mpuInterruptCallback);
00227 
00228     RfChip.PCD_Init();
00229     wait(0.2);
00230 
00231 
00232 
00233 
00234 
00235 
00236     ////////////////////MPU INTERUPT RADI//////////////////////////////////////
00237 
00238     mpuInterrupt.fall(mpuInterruptCallback);
00239     wait(0.2);
00240     mpu.calibrate(accelBias, gyroBias);
00241     mpu.initialize();
00242     mpu.setSleepEnabled(0);
00243     wait(0.2);
00244     mpu.setIntFreefallEnabled(1);
00245     mpu.setIntZeroMotionEnabled(1);
00246     mpu.setIntMotionEnabled(1);
00247     printf("Setting mpu parameters...\r\n");
00248     mpu.setMotionDetectionThreshold(1);
00249     mpu.setMotionDetectionDuration(15);
00250     mpu.setZeroMotionDetectionThreshold(1);
00251     mpu.setZeroMotionDetectionDuration(5);
00252 
00253 
00254     /////////////////////////////////////////////////////////////////////////////
00255     /*
00256      mpu.calibrate(accelBias, gyroBias);
00257     mpu.initialize();
00258     mpu.setSleepEnabled(0);
00259     mpu.setIntDMPEnabled(1);
00260 
00261     uint8_t pinValue = mpu.getIntEnabled();
00262     printf("Is int enabled? : %d\n", pinValue);
00263     printf("MPU6050 testing connection \n");
00264 
00265 
00266     wait(2);
00267     bool mpu6050TestResult = mpu.testConnection();
00268     if(mpu6050TestResult) {
00269         printf("MPU6050 test passed \n");
00270     } else {
00271         printf("MPU6050 test failed \n");
00272     }
00273 
00274 
00275     mpu.setIntEnabled(1);
00276     //mpu.setIntFreefallEnabled(1);
00277     //mpu.setIntZeroMotionEnabled(0);
00278     //mpu.setIntMotionEnabled(1);
00279     mpu.setDHPFMode(1);
00280 
00281     //*******************************************************************************************************************************
00282 
00283     printf("\n\n\n");
00284     uint8_t motionDetectionThreshold = mpu.getMotionDetectionThreshold();
00285     //printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);
00286     //printf("MPU6050 set motion detection threshold to 2\r\n");
00287     mpu.setMotionDetectionThreshold(1);
00288     motionDetectionThreshold = mpu.getMotionDetectionThreshold();
00289     //printf("MPU6050 motion detection threshold now is: %d \r\n", motionDetectionThreshold);
00290     printf("Motion detection threshold is: %d \r\n", motionDetectionThreshold);
00291     uint8_t motionDetectionDuration = mpu.getMotionDetectionDuration();
00292     //printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
00293     //printf("MPU6050 set motion detection duration to 40\r\n");
00294     mpu.setMotionDetectionDuration(20);
00295     motionDetectionDuration = mpu.getMotionDetectionDuration();
00296     //printf("MPU6050 motion detection duration now is: %d \r\n", motionDetectionDuration);
00297     printf("Motion detection duration is: %d \r\n", motionDetectionDuration);
00298 
00299     //*******************************************************************************************************************************
00300 
00301     //printf("\n\n\n");
00302     uint8_t zeroMotionDetectionThreshold = mpu.getZeroMotionDetectionThreshold();
00303     //printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
00304     //printf("MPU6050 set zero motion detection threshold to 2\r\n");
00305     mpu.setZeroMotionDetectionThreshold(2);
00306     zeroMotionDetectionThreshold = mpu.getZeroMotionDetectionThreshold();
00307     //printf("MPU6050 zero motion detection threshold now is: %d \r\n", zeroMotionDetectionThreshold);
00308     printf("Zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
00309     //printf("\n\n\n");
00310     int8_t zeroMotionDetectionDuration = mpu.getZeroMotionDetectionDuration();
00311     //printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
00312     //printf("MPU6050 set zero motion detection duration to 1\r\n");
00313     mpu.setZeroMotionDetectionDuration(1);
00314     zeroMotionDetectionDuration = mpu.getZeroMotionDetectionDuration();
00315     //printf("MPU6050 zero motion detection duration now is: %d \r\n", zeroMotionDetectionDuration);
00316     printf("Zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
00317 
00318     //*******************************************************************************************************************************
00319 
00320     //printf("\n\n\n");
00321     int8_t freefallDetectionThreshold = mpu.getFreefallDetectionThreshold();
00322     //printf("MPU6050 freefall motion detection threshold is: %d \r\n", freefallDetectionThreshold);
00323     //printf("MPU6050 set freefall motion detection threshold to 40\r\n");
00324     //mpu.setFreefallDetectionThreshold(2);
00325     freefallDetectionThreshold = mpu.getFreefallDetectionThreshold();
00326     //printf("MPU6050 freefall motion detection threshold now is: %d \r\n", freefallDetectionThreshold);
00327     printf("Freefall motion detection threshold is: %d \r\n", freefallDetectionThreshold);
00328     //printf("\n\n\n");
00329     int8_t freefallDetectionDuration = mpu.getFreefallDetectionDuration();
00330     //printf("MPU6050 freefall motion detection duration is: %d \r\n", freefallDetectionDuration);
00331     //printf("MPU6050 set freefall motion detection duration to 40\r\n");
00332     //mpu.setFreefallDetectionDuration(40);
00333     freefallDetectionDuration = mpu.getFreefallDetectionDuration();
00334     //printf("MPU6050 freefall motion detection duration now is: %d \r\n", freefallDetectionDuration);
00335     printf("Freefall motion detection duration is: %d \r\n", freefallDetectionDuration);
00336     printf("\n\n\n");
00337     printf("MPU6050 test\n\n");
00338 
00339 
00340     //*******************************************************************************************************************************
00341     */
00342 
00343 
00344 
00345     while(1) {
00346 
00347         if(test==1) {
00348             wait(0.2);
00349             test = 0;
00350             getXYZMotions();
00351             //mot=2;
00352         }
00353 
00354 
00355 
00356         if(print==1) {
00357             mojlogger.print();
00358             print=0;
00359         }
00360 
00361 
00362 
00363 //printf("caseFsm->DeviceInFullState()=%d\n",caseFsm->DeviceInFullState());
00364         //This goes into caseFsm, but for now we are testing in main.cpp
00365         if(caseFsm->DeviceInFullState()) {
00366             printf("Waiting 5 seconds for RFID event...\r\n");
00367             while (caseFsm->DeviceInFullState()) {
00368                 // Look for new cards
00369 
00370 
00371                 if ( ! RfChip.PICC_IsNewCardPresent()) {
00372                     wait_ms(500);
00373                     printf("!RfChip.PICC_IsNewCardPresent()\n");
00374                     continue;
00375                 }
00376 
00377                 // Select one of the cards
00378                 if ( ! RfChip.PICC_ReadCardSerial()) {
00379                     printf("!RfChip.PICC_ReadCardSerial()\n");
00380                     wait_ms(500);
00381                     continue;
00382                 }
00383 
00384                 // Print Card UID
00385                 printf("Card UID: ");
00386                 for (uint8_t i = 0; i < RfChip.uid.size; i++) {
00387                     rfidEvent.uid[i] = RfChip.uid.uidByte[i];
00388                     printf("%X ", RfChip.uid.uidByte[i]);
00389                 }
00390                 rfidEvent.size = RfChip.uid.size;
00391                 printf("\n\r");
00392 
00393                 // Print Card type
00394                 uint8_t piccType = RfChip.PICC_GetType(RfChip.uid.sak);
00395                 printf("PICC Type: %s \n\r", RfChip.PICC_GetTypeName(piccType));
00396                 wait_ms(1000);
00397                 rfidEvent.cardType = piccType;
00398                 //phone inserted
00399                 caseFsm->handle(rfidEvent);
00400                 printf("_%d_\n",caseFsm->DeviceInPhoneDetectedState());
00401                 if(caseFsm->DeviceInPhoneDetectedState()) {
00402                     wait(0.3);
00403                     x++;
00404                     printf("snimi\n");
00405                     mojlog.setLog(600,1,0,600);
00406                     mojlogger.savelog(mojlog);
00407                     ;//LOGOVATI INSERTED
00408 
00409                 }
00410 
00411             }
00412             if(caseFsm->DeviceInDeviceEmptyState()) {
00413                 wait(0.3);
00414                 printf("ne snimi\n");
00415                 x++;   //LOGOVATI REMOVED
00416                 mojlog.setLog(400,0,0,400);
00417                 mojlogger.savelog(mojlog);
00418             }
00419         }
00420 
00421 
00422 
00423     }
00424 
00425 }