Diff: source/main.cpp

Revision:

4:147bbe6f9626

Parent:

3:b2087af18efe

Child:

5:ea629a3fd6c1

--- a/source/main.cpp	Wed Jun 21 16:00:36 2017 +0000
+++ b/source/main.cpp	Wed Aug 23 17:26:31 2017 +0000
@@ -5,9 +5,28 @@
 #include "ble/services/BatteryService.h"
 #include "MPU6050.h"
 #include "MPUService.h"
-#include <string> 
+#include <string>
 #include <stdio.h>
 #include "SoftSerial.h"
+#include "circular_buffer.h"
+#include <Fsm.h>
+#include <Events.h>
+bool test=0;
+InterruptIn mpuInterrupt(p5);
+struct TurnOff turnoff;
+struct TurnOn turnon;
+struct Move move;
+
+Fsm* fsm;
+
+struct MotionLog {
+    int motX;
+    int motY;
+    int motZ;
+};
+MotionLog motionLog;
+circular_buffer<MotionLog> cb(5);
+
 Serial pc(p6, p8);
 //SoftSerial pc1(p9, p10);
 MPU6050 mpu6050;
@@ -17,6 +36,33 @@
 uint16_t customServiceUUID  = 0xA000;
 uint16_t readCharUUID       = 0xA001;
 uint16_t interruptUUID      = 0xA002;
+uint16_t gyroUUID       = 0xA003;
+
+
+
+uint16_t motionThresholdUUID      = 0xA004;
+static uint8_t motionThresholdValue[10] = {0};
+WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(motionThresholdValue)> motionThreshold(motionThresholdUUID, motionThresholdValue);
+
+
+uint16_t motionDurationUUID      = 0xA005;
+static uint8_t motionDurationValue[10] = {0};
+WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(motionDurationValue)> motionDuration(motionDurationUUID, motionDurationValue);
+
+
+uint16_t zeroMotionThresholdUUID      = 0xA006;
+static uint8_t zeroMotionThresholdValue[10] = {0};
+WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(zeroMotionThresholdValue)> zeroMotionThreshold(zeroMotionThresholdUUID, zeroMotionThresholdValue);
+
+
+uint16_t zeroMotionDurationUUID      = 0xA007;
+static uint8_t zeroMotionDurationValue[10] = {0};
+WriteOnlyArrayGattCharacteristic<uint8_t, sizeof(zeroMotionDurationValue)> zeroMotionDuration(zeroMotionDurationUUID, zeroMotionDurationValue);
+
+uint16_t fsmStateUUID       = 0xA008;
+static float fsmStateValue[3] = {0};
+ReadOnlyArrayGattCharacteristic<float, sizeof(fsmStateValue)> fsmState(fsmStateUUID, fsmStateValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
+
 DigitalOut alivenessLED(LED1, 0);
 const static char     DEVICE_NAME[] = "MPU";
 static const uint16_t uuid16_list[] = {0x0A0A};
@@ -24,14 +70,18 @@
 /* Set Up custom Characteristics */
 static float readValue[3] = {0};
 static float interruptValue[3] = {0};
+static float gyroValue[3] = {0};
+
+
 ReadOnlyArrayGattCharacteristic<float, sizeof(readValue)> readChar(readCharUUID, readValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
 ReadOnlyArrayGattCharacteristic<float, sizeof(interruptValue)> interrupt(interruptUUID, interruptValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
+ReadOnlyArrayGattCharacteristic<float, sizeof(gyroValue)> gyro(gyroUUID, gyroValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
 
 //static float writeValue[2] = {3,4};
 //WriteOnlyArrayGattCharacteristic<float, sizeof(writeValue)> writeChar(writeCharUUID, writeValue);
 
 /* Set up custom service */
-GattCharacteristic *characteristics[] = {&readChar/*, &writeChar*/, &interrupt};
+GattCharacteristic *characteristics[] = {&readChar/*, &writeChar*/, &interrupt, &gyro, &motionThreshold, &motionDuration, &zeroMotionThreshold, &zeroMotionDuration, &fsmState};
 GattService        customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
 
 
@@ -42,40 +92,157 @@
 
 
 
-void updateReadValue() {
-    alivenessLED = !alivenessLED; 
-    mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-    
-    
-    //mpu6050.readAccelData(accelData);
-    //mpu6050.getAres();
-    //ax = accelData[0] * aRes - accelBias[0];
-    //ay = accelData[1] * aRes - accelBias[1];
-    //az = accelData[2] * aRes - accelBias[2];
-    float newValue[3] = {ax,ay,az};
-    //mpu6050.readGyroData(gyroData);
-    //mpu6050.getGres();
-    
-    //gx = gyroData[0] * gRes; // - gyroBias[0];
-    //gy = gyroData[1] * gRes; // - gyroBias[1];
-    //gz = gyroData[2] * gRes; // - gyroBias[2];
-    //float newValue[3] = {gx,gy,gz};*/
+void updateReadValue()
+{
+    alivenessLED = !alivenessLED;
+    //mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+    //mpu6050.getAcceleration(&ax, &ay, &az);
+
+    mpu6050.readAccelData(accelData);
+    mpu6050.getAres();
+    ax = accelData[0]*aRes  - accelBias[0];
+    ay = accelData[1]*aRes  - accelBias[1];
+    az = accelData[2]*aRes  - accelBias[2];
+    float accelValues[3] = {ax,ay,az};
+
+    mpu6050.readGyroData(gyroData);
+    mpu6050.getGres();
+    gx = gyroData[0]*gRes - gyroBias[0];
+    gy = gyroData[1]*gRes - gyroBias[1];
+    gz = gyroData[2]*gRes - gyroBias[2];
+    float gyroValues[3] = {gx,gy,gz};
+
     //counter++;
     //pc.printf("| Before | ax=%.3f | ay=%.3f | az=%.3f \r\n",gx,gy,gz);
-    pc.printf("| Acce | ax=%.3f | ay=%.3f | az=%.3f | Gyro | gx=%.3f | gy=%.3f | gz=%.3f\r\n",ax,ay,az,gx,gy,gz);
+    //pc.printf("| Acce | ax=%.3f | ay=%.3f | az=%.3f | Gyro | gx=%.3f | gy=%.3f | gz=%.3f\r\n",ax,ay,az,gx,gy,gz);
     //len_send = strlen((const char *)newValue);
-    
+
 
     uint8_t motionByte = mpu6050.readThisByte(MPU6050_RA_MOT_DETECT_STATUS);
-    pc.printf("Motion byte: %d\r\n",motionByte);
-    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(interrupt.getValueHandle(), (uint8_t*)motionByte, 8);
-    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)newValue, 12);
+    uint8_t b;
+    bool zero = false;
+    float fx, fy,fz=0;
+    //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) {
+            zero = true;
+            //pc.printf("    %c    %c    %c     |    ",x,y,z);
+            //pc.printf(" | %.3f | %.3f | %.3f | %.3f | %.3f | %.3f  ZERO DETECTED\r\n",ax,ay,az, gx, gy, gz);
+            //pc.printf(" | %d | %d | %d | %d | %d | %d  ZERO DETECTED\r\n",ax,ay,az, gx, gy, gz);
+        }
+        if(i==1 && b==2) {
+            //pc.printf("This should not happen :).\r\n");
+        }
+        if(i==6 && b==64) {
+            //x='+';
+            fx=1;
+            mx=1;
+            //pc.printf("X POSITIVE motion detected.\r\n");
+        }
+        if(i==7 && b==128) {
+            //x='-';
+            fx=2;
+            mx=1;
+            //pc.printf("X NEGATIVE motion detected.\r\n");
+        }
+        if(i==4 && b==16) {
+            //y='+';
+            fy=1;
+            my=1;
+            //pc.printf("Y POSITIVE motion detected.\r\n");
+        }
+        if(i==5 && b==32) {
+            //y='-';
+            fy=2;
+            my=1;
+            //pc.printf("Y NEGATIVE motion detected.\r\n");
+        }
+        if(i==2 && b==4) {
+            //z='+';
+            fz=1;
+            mz=1;
+            //pc.printf("Z POSITIVE motion detected.\r\n");
+        }
+        if(i==3 && b==8) {
+            //z='-';
+            fz=2;
+            mz=1;
+            //pc.printf("Z NEGATIVE motion detected.\r\n");
+        }
+    }
+   // if(test) {
+        motionLog.motX = mx;
+        motionLog.motY = my;
+        motionLog.motZ = mz;
+
+        cb.push_back(motionLog);
+        //pc.printf("Desio se motion\r\n");
+      //  test=0;
+    //}
+
+    int numX, numY, numZ=0;
+    //pc.printf("\r\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
+    numX=numY=numZ=0;
+    for(int i=0; i<=4; i++) {
+        //pc.printf("%d %d %d\r\n",cb[i].motX, cb[i].motY, cb[i].motZ);
+        if(cb[i].motX!=0) {
+            numX++;
+        }
+        if(cb[i].motY!=0) {
+            numY++;
+        }
+        if(cb[i].motZ!=0) {
+            numZ++;
+        }
+    }
+    if(numX<=2) {
+        numX=0;
+    } else
+        numX=1;
+
+    if(numY<=2) {
+        numY=0;
+    } else
+        numY=1;
+
+    if(numZ<=2) {
+        numZ=0;
+    } else
+        numZ=1;
+    
+    if(numX!=0 || numY!=0 || numZ!=0){
+        numX=1;
+        numY=1;
+        numZ=1;
+        }
+
+
+    //pc.printf("***************************\r\n");
+    //pc.printf("%d %d %d\r\n",numX,numY,numZ);
+    float fsmStateValues[3] = {numX,numY,numZ};
+    if(!zero) {
+        //pc.printf("    %c    %c    %c     |    ",x,y,z);
+        //pc.printf(" | %.3f | %.3f | %.3f | %.3f | %.3f | %.3f\r\n",ax,ay,az, gx, gy, gz);
+        //pc.printf(" | %d | %d | %d | %d | %d | %d\r\n",ax,ay,az, gx, gy, gz);
+    }
+
+    float motionValues[3] = {fx,fy,fz};
+
+    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(interrupt.getValueHandle(), (uint8_t*)motionValues, 12);
+    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)accelValues, 12);
+    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(gyro.getValueHandle(), (uint8_t*)gyroValues, 12);
+    BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(fsmState.getValueHandle(), (uint8_t*)fsmStateValues, 12);
 }
 void periodicCallback(void)
 {
-    if (BLE::Instance().getGapState().connected) {
-        eventQueue.call(updateReadValue);
-    }
+    //if (BLE::Instance().getGapState().connected) {
+    //    eventQueue.call(updateReadValue);
+    //}
 }
 
 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context)
@@ -93,7 +260,57 @@
 {
     /* Initialization error handling should go here */
 }
+void writeCharCallback(const GattWriteCallbackParams *params)
+{
+    printf("Nesto ima na ulazu\r\n");
+    printf("Handle je: %d\r\n", params->handle);
+    if(params->handle == motionThreshold.getValueHandle()) {
+        uint8_t value = params->data[0];
+        printf("Value is: %d\r\n", value);
+        mpu6050.setMotionDetectionThreshold(value);
+        uint8_t motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
+        pc.printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);
 
+        //printf("Data received: length = %d, data = 0x",params->len);
+
+
+        // for(int x=0; x < params->len; x++) {
+        //     printf("%x", params->data[x]);
+        //  }
+        // printf("\n\r");
+    }
+
+    if(params->handle == motionDuration.getValueHandle()) {
+        uint8_t value = params->data[0];
+        printf("Value is: %d\r\n", value);
+        mpu6050.setMotionDetectionDuration(value);
+        uint8_t motionDetectionDuration = mpu6050.getMotionDetectionDuration();
+        pc.printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
+        //printf("Data received: length = %d, data = 0x",params->len);
+
+
+        // for(int x=0; x < params->len; x++) {
+        //     printf("%x", params->data[x]);
+        //  }
+        // printf("\n\r");
+    }
+    if(params->handle == zeroMotionThreshold.getValueHandle()) {
+        uint8_t value = params->data[0];
+        printf("Value is: %d\r\n", value);
+        mpu6050.setZeroMotionDetectionThreshold(value);
+        uint8_t zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
+        pc.printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
+
+    }
+    if(params->handle == zeroMotionDuration.getValueHandle()) {
+        uint8_t value = params->data[0];
+        printf("Value is: %d\r\n", value);
+        mpu6050.setZeroMotionDetectionDuration(value);
+        uint8_t zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
+        pc.printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
+
+    }
+}
 
 void bleInitComplete(BLE::InitializationCompleteCallbackContext* params)
 {
@@ -111,52 +328,113 @@
         return;
     }
     ble.gap().onDisconnection(disconnectionCallback);
-
+    ble.gattServer().onDataWritten(writeCharCallback);
     /* setup advertising */
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t*)DEVICE_NAME, sizeof(DEVICE_NAME));
     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
-    
+
     ble.gap().startAdvertising();
     ble.addService(customService);
     ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
-    
+
 
 }
-void flip() {
+void flip()
+{
     //pc.printf("Desio se interupt");
 }
+void mpuInterruptCallback()
+{
+    //test=1;
+    
+    if (BLE::Instance().getGapState().connected) {
+        eventQueue.call(updateReadValue);
+    }
+
+}
 bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD;
 int main()
-{  
+{
     //mpu6050.whoAmI();
     //mpu6050.calibrate(accelBias, gyroBias);
     //mpu6050.init();
+
+    motionLog.motX=0;
+    motionLog.motY=0;
+    motionLog.motZ=0;
+    for(int i=0; i<=4; i++) {
+        cb.push_back(motionLog);
+    }
+
+
     mpu6050.calibrate(accelBias, gyroBias);
     mpu6050.initialize();
     mpu6050.setSleepEnabled(0);
     pc.baud(115200);
+    mpuInterrupt.fall(mpuInterruptCallback);
+    
+    //mpu6050.setIntEnabled(1);
+    mpu6050.setIntZeroMotionEnabled(1);
+    mpu6050.setIntMotionEnabled(1);
     mpu6050.setDHPFMode(1);
+
+    pc.printf("\n\n\n");
+    uint8_t motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
+    //pc.printf("MPU6050 motion detection threshold is: %d \r\n", motionDetectionThreshold);
+    //pc.printf("MPU6050 set motion detection threshold to 2\r\n");
+    mpu6050.setMotionDetectionThreshold(1);
+    motionDetectionThreshold = mpu6050.getMotionDetectionThreshold();
+    //pc.printf("MPU6050 motion detection threshold now is: %d \r\n", motionDetectionThreshold);
+    pc.printf("Motion detection threshold is: %d \r\n", motionDetectionThreshold);
+    uint8_t motionDetectionDuration = mpu6050.getMotionDetectionDuration();
+    //pc.printf("MPU6050 motion detection duration is: %d \r\n", motionDetectionDuration);
+    //pc.printf("MPU6050 set motion detection duration to 40\r\n");
+    mpu6050.setMotionDetectionDuration(20);
+    motionDetectionDuration = mpu6050.getMotionDetectionDuration();
+    //pc.printf("MPU6050 motion detection duration now is: %d \r\n", motionDetectionDuration);
+    pc.printf("Motion detection duration is: %d \r\n", motionDetectionDuration);
+
+    //*******************************************************************************************************************************
+
+    //pc.printf("\n\n\n");
+    uint8_t zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
+    //pc.printf("MPU6050 zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
+    //pc.printf("MPU6050 set zero motion detection threshold to 2\r\n");
+    mpu6050.setZeroMotionDetectionThreshold(2);
+    zeroMotionDetectionThreshold = mpu6050.getZeroMotionDetectionThreshold();
+    //pc.printf("MPU6050 zero motion detection threshold now is: %d \r\n", zeroMotionDetectionThreshold);
+    pc.printf("Zero motion detection threshold is: %d \r\n", zeroMotionDetectionThreshold);
+    //pc.printf("\n\n\n");
+    int8_t zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
+    //pc.printf("MPU6050 zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
+    //pc.printf("MPU6050 set zero motion detection duration to 1\r\n");
+    mpu6050.setZeroMotionDetectionDuration(1);
+    zeroMotionDetectionDuration = mpu6050.getZeroMotionDetectionDuration();
+    //pc.printf("MPU6050 zero motion detection duration now is: %d \r\n", zeroMotionDetectionDuration);
+    pc.printf("Zero motion detection duration is: %d \r\n", zeroMotionDetectionDuration);
+
+    //*******************************************************************************************************************************
     //setMotionDetectionThreshold
     //char myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
     //pc.printf("Prije postavljanja\n");
     //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
-    //pc.printf("Poslije postavljanja\n");      
+    //pc.printf("Poslije postavljanja\n");
     //mpu6050.writeByte(MPU6050_ADDRESS, 0x1F,0x02);
     //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
     //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
-    
-    
+
+
     //setMotionDetectionDuration
     //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
     //pc.printf("Prije postavljanja\n");
     //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
-    //pc.printf("Poslije postavljanja\n");   
+    //pc.printf("Poslije postavljanja\n");
     //mpu6050.writeByte(MPU6050_ADDRESS, 0x20,0x28);
     //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
     //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
-    
+
 //    XnegMD = mpu6050.getXNegMotionDetected();
     //mpu6050.whoAmI();
     //uint8_t *data;
@@ -165,29 +443,35 @@
     //pc.printf("Byte iznosi: %x\n",byte);
     //int i = data[0];
     //pc.printf("7 bit iznosi: %d\n", i);
-    
+
     //XposMD = mpu6050.getXPosMotionDetected();
-   
-    
+
+
     //pin.fall(&flip);
     eventQueue.call_every(500, periodicCallback);
     BLE& ble = BLE::Instance();
+    fsm = new Fsm(ble);
     ble.onEventsToProcess(scheduleBleEventsProcessing);
-    ble.init(bleInitComplete);  
+    ble.init(bleInitComplete);
     eventQueue.dispatch_forever();
-    while (ble.hasInitialized()  == false) { /* spin loop */ }
-  
-  
-  
-  
-  
-  
-   // pc1.baud(9600);
+
+
+    while (ble.hasInitialized()  == false) {
+        /* spin loop */
+
+    }
+
+
+
+
+
+
+    // pc1.baud(9600);
     //while (true) {
-        //ble.waitForEvent();
-      //  pc1.printf("tare ");
-        //wait_ms(1000);
-        
+    //ble.waitForEvent();
+    //  pc1.printf("tare ");
+    //wait_ms(1000);
+
     //}
     //return 0;
 }