Diff: source/main.cpp

Revision:

5:ea629a3fd6c1

Parent:

4:147bbe6f9626

Child:

7:bc915651d90e

--- a/source/main.cpp	Wed Aug 23 17:26:31 2017 +0000
+++ b/source/main.cpp	Thu Nov 22 17:46:57 2018 +0000
@@ -1,477 +1,79 @@
-#include <events/mbed_events.h>
+//#include <events/mbed_events.h>
 #include <mbed.h>
-#include "ble/BLE.h"
-#include "ble/Gap.h"
-#include "ble/services/BatteryService.h"
 #include "MPU6050.h"
-#include "MPUService.h"
 #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;
-//InterruptIn pin(p10);
-//DigitalOut led(LED1, 1);
-//DigitalOut led2(LED2, 1);
-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 led(p25, 1);
+MPU6050 mpu6050;
+Serial pc(USBTX,USBRX);
 
-DigitalOut alivenessLED(LED1, 0);
-const static char     DEVICE_NAME[] = "MPU";
-static const uint16_t uuid16_list[] = {0x0A0A};
-//static int counter;
-/* 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, &gyro, &motionThreshold, &motionDuration, &zeroMotionThreshold, &zeroMotionDuration, &fsmState};
-GattService        customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
-
-
-
-static EventQueue eventQueue(/* event count */ 10 * /* event size */ 32);
-InterruptIn button(BUTTON1);
-
-
-
-
-void updateReadValue()
+int main()
 {
-    alivenessLED = !alivenessLED;
-    //mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-    //mpu6050.getAcceleration(&ax, &ay, &az);
-
+    pc.baud(115200);
+    pc.printf("START_1\n");
+    mpu6050.calibrate(accelBias, gyroBias);
+    mpu6050.initialize();
+    pc.printf("START_2\n");
+    
+   
+ 
+ /////////////////////////////////////////////////////////SERIAL////////////////////////////////////////////
+    while(0){
+        led = 0;
+        wait(1);
+   if(pc.readable()){
+       if(pc.getc()=='.'){
+            pc.putc(pc.getc());
+            led=1;
+          wait(1);
+          }
+            }
+  
+        }
+       
+ /////////////////////////////////////////////////////////SERIAL/////////////////////////////////////////////
+ 
+ /////////////////////////////////////////////////////////MPU6050////////////////////////////////////////////
+ 
+    while(0){
+         pc.printf("i am %d\n",mpu6050.getDeviceID());
+         wait(1);
+        }
+       
+ /////////////////////////////////////////////////////////MPU6050/////////////////////////////////////////////
+ 
+ 
+ 
+ //////////////////////////////////////////////////////////////GET_DATA////////////////////////////////////////////
+    while(1){
+ 
     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);
-    //len_send = strlen((const char *)newValue);
-
-
-    uint8_t motionByte = mpu6050.readThisByte(MPU6050_RA_MOT_DETECT_STATUS);
-    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;
+        
+    if(pc.readable())
+    {
+        if(pc.getc()=='.'){
+            
+            pc.printf("%f, ",gx);
+            pc.printf("%f, ",gy);
+            pc.printf("%f",gz);
+           
         }
 
-
-    //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);
-    //}
-}
-
-void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context)
-{
-    BLE& ble = BLE::Instance();
-    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
-}
-
-void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
-{
-    (void) params;
-    BLE::Instance().gap().startAdvertising();
-}
-void onBleInitError(BLE &ble, ble_error_t error)
-{
-    /* 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)
-{
-    BLE& ble = params->ble;
-    ble_error_t error = params->error;
-
-    if (error != BLE_ERROR_NONE) {
-        /* In case of error, forward the error handling to onBleInitError */
-        onBleInitError(ble, error);
-        return;
-    }
-
-    /* Ensure that it is the default instance of BLE */
-    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
-        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()
-{
-    //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");
-    //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");
-    //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;
-    //char byte = mpu6050.readByte(MPU6050_ADDRESS, 0x61);
-    //*data = byte & (1 << 7);
-    //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);
-    eventQueue.dispatch_forever();
-
-
-    while (ble.hasInitialized()  == false) {
-        /* spin loop */
-
-    }
-
-
-
-
-
-
-    // pc1.baud(9600);
-    //while (true) {
-    //ble.waitForEvent();
-    //  pc1.printf("tare ");
-    //wait_ms(1000);
-
-    //}
-    //return 0;
-}