Revision 8:144855fe02bd, committed 2016-02-12

Comitter:

fabiombed

Date:

Fri Feb 12 10:52:50 2016 +0000

Parent:

7:54984d031243

Child:

9:ca289bf57f52

Commit message:

some fix

Changed in this revision

--- a/CustomControlService.h	Wed Feb 10 12:52:55 2016 +0000
+++ b/CustomControlService.h	Fri Feb 12 10:52:50 2016 +0000
@@ -75,7 +75,27 @@
     }
 
 // Tests Method
-//----------------INIZIO  
+
+    void enNotify (Gap::Handle_t handle) {
+            if (isContHandle(handle)) { 
+                PRINTF("enNotify! %d\n\r", handle); isEnabledControlNotify = true; return; }    
+    }
+    
+    void disNotify (Gap::Handle_t handle) {
+            if (isContHandle(handle)) { 
+                            isEnabledControlNotify = false; return; }    
+    }   
+    
+    bool isContHandle (Gap::Handle_t handle) {
+            if (handle == userbuttonCharacteristic.getValueAttribute().getHandle()) return true;
+            return false;
+    }
+    
+    void updateConnectionStatus(ConnectionStatus_t status) {            
+            isEnabledControlNotify = false;
+            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);             
+    }      
+
 /*
   
     uint32_t sendcontrolState(uint32_t Feature, uint8_t Command, uint8_t val, uint16_t TimeStamp) {      
@@ -93,28 +113,7 @@
             return 0;
     }
 
-    void updateConnectionStatus(ConnectionStatus_t status) {            
-            isEnabledControlNotify = false;
-            memset (controlData, 0, SIZEOF_CONTROL_DATA_LEN);             
-            }   
-    
-    bool isConfHandle (Gap::Handle_t handle) {
-            if (handle == userbuttonCharacteristic.getValueAttribute().getHandle()) return true;
-            return false;
-    }           
-
-    void enNotify (Gap::Handle_t handle) {
-            if (isConfHandle(handle)) { 
-                PRINTF("enNotify! %d\n\r", handle); isEnabledControlNotify = true; return; }    
-    }
-    
-    void disNotify (Gap::Handle_t handle) {
-            if (isConfHandle(handle)) { 
-                            isEnabledControlNotify = false; return; }    
-    }   
-
 */    
-//----------------FINE  
 
 // Variables Initialization
 private:

--- a/CustomSensorsService.h	Wed Feb 10 12:52:55 2016 +0000
+++ b/CustomSensorsService.h	Fri Feb 12 10:52:50 2016 +0000
@@ -79,7 +79,6 @@
     }
 
 // Tests Method
-//----------------INIZIO  
 
     void sendEnvPosition (int16_t Pos, uint16_t TimeStamp) {
             STORE_LE_16(envPosition,TimeStamp);
@@ -89,7 +88,6 @@
             ble.gattServer().write(positionCharacteristic.getValueAttribute().getHandle(), envPosition, POSITION_DATA_LEN, 0);         
     } 
 
-
 /*
 
     void sendEnvTemperature (int16_t Temp, uint16_t TimeStamp) {
@@ -190,9 +188,7 @@
     } 
     
 */    
-//----------------FINE  
-                                  
-        
+                                        
     void enNotify (Gap::Handle_t handle) {
             if (isPositionHandle(handle)) { isEnabledPositionNotify = true; memset(envPosition,0,POSITION_DATA_LEN); return; }
             if (isSunpanelHandle(handle)) { isEnabledSunpanelNotify = true; memset(envSunpanel,0,SUNPANEL_DATA_LEN); return; }                     

--- a/main.cpp	Wed Feb 10 12:52:55 2016 +0000
+++ b/main.cpp	Fri Feb 12 10:52:50 2016 +0000
@@ -61,6 +61,10 @@
 #include "debug.h" // Need for PRINTF
 #include "Utils.h" // Need for STORE_LE_16 and _32
 
+const unsigned   LENGTH_OF_LONG_UUID = 16;
+typedef uint16_t ShortUUIDBytes_t;
+typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
+
 typedef struct {
     int32_t AXIS_X;
     int32_t AXIS_Y;
@@ -70,21 +74,16 @@
 typedef enum ConnectionStatus_t {
     DISCONNECTED    =0,
     CONNECTED       =1
-}cns_t;
+} cns_t;
 
-const unsigned   LENGTH_OF_LONG_UUID = 16;
-typedef uint16_t ShortUUIDBytes_t;
-typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
+#define BLE_DEV_NAME "SunTracker"
+#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF
+#define BLE_ADVERTISING_INTERVAL 1000
 
 #include "CustomControlService.h"
 #include "CustomSensorsService.h"
 
 static BLE *p_BLEdev = NULL;
-
-#define BLE_DEV_NAME "SunTracker"
-#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF
-#define BLE_ADVERTISING_INTERVAL 1000
-
 static CustomControlService *p_customcontrolservice = NULL;
 static CustomSensorService  *p_customsensorservice = NULL;
 
@@ -103,6 +102,7 @@
 
 int16_t dir=0;          // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
 int16_t changedir=0;    // Change Direction: 0 = No, 1 = Yes
+int16_t arrest=0;       // Arrest: 0 = No, 1 = Yes
 int16_t babybear=0;     // Difference (in Lux) between Left and Right
 int acc_data[3];        // Difference of Accelerometer
 int16_t diff=0;         // Abs of Babybear or Accelerometer difference
@@ -114,12 +114,200 @@
 int16_t Display=0;      // Shown on Display: 0 = Motor Speed, 1 = Solar Panel Value, 2 = Manual Control
 int16_t status, status_t, status_b, status_l, status_r; // Babybear Status
 
-/* ---------------------------------------------------------------------------*/  
+/* Initializations ------------------------------------------------------------*/
+
+// Initializing SPI bus
+DevSPI dev_spi(D11, D12, D13);
+
+// Initializing I2C bus
+DevI2C dev_i2c(D14, D15);
+
+// Initializing Motor Component IHM01A1
+static L6474 *motor;
+
+// Initializing Babybear Component 6180XA1
+static X_NUCLEO_6180XA1 *board;
+MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
+    
+// Initializing MEMS Component IKS01A1
+static X_NUCLEO_IKS01A1 *mems;
+MotionSensor *accelerometer;
+
+InterruptIn mybutton(USER_BUTTON);
+
+//AnalogIn analog_read(A1); // A1 Conflict with BLE SPI_CS --> Changed it!!!
+
+/* User_Button_Pressed -------------------------------------------------------*/
+
+void User_Button_Pressed()
+{
+
+    if (start>0)    { Display++; }
+    if (Display>2)  { Display=0; }
+    if (start==0)   { start=1; }
+    
+    printf("PUSH Display %d\r\n", Display);
+
+}
+
+/* Initialization ------------------------------------------------------------*/
+
+bool Initialization(void)
+{
+  
+    // Initializing MEMS Component
+    mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
+    accelerometer = mems->GetAccelerometer();
+   
+//----
+
+    // Initializing Babybear Component
+    //board=X_NUCLEO_6180XA1::Instance(&dev_i2c);
+    board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
+
+    //status=board->InitBoard();
+    //if(status) VL6180x_ErrLog("Failed to init the board!\n\r");
+
+    // Put GPIO not used as Interrupt in Hi-Z
+    status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); No Present
+    status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    
+    // Set Babybears
+    status_l=board->sensor_left->AlsSetAnalogueGain(3);
+    status_r=board->sensor_right->AlsSetAnalogueGain(3);
+    status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
+    status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
+
+//----
+
+    // Initializing Motor Component
+    motor = new L6474(D2, D8, D7, D9, D10, dev_spi); // D7 conflict with BNRG_RST (when motor change direction 0-1 reset BLE)
+    if (motor->Init(NULL) != COMPONENT_OK)
+        return false;
+
+    motor->SetStepMode(STEP_MODE_1_8);  // Default is STEP_MODE_1_16
+
+    // Set defaults Motor Speed
+    motor->SetAcceleration(SET_ACC);
+    motor->SetDeceleration(SET_DEC);
+    motor->SetMaxSpeed(SET_MAX);        // Variable by Light/Mems Sensors
+    motor->SetMinSpeed(SET_MIN);
+
+    return true;
+
+}
+
+/* Measure_Babybear ----------------------------------------------------------*/
+
+void Measure_Babybear(void)
+{
+
+    status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left);
+    status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right);
+
+    babybear = data_sensor_right.lux - data_sensor_left.lux;
+
+    diff = abs(babybear);
 
-//void DISP_ExecLoopBody(void) {};
+    if (babybear>0) { left=0; right=1; }
+    if (babybear<0) { left=1; right=0; }
+
+}
+
+/* Measure_Accelerometer -----------------------------------------------------*/
+
+void Measure_Accelerometer(void)
+{
+
+    accelerometer->Get_X_Axes(acc_data);
+
+    diff = abs(acc_data[0]);
+
+    if (acc_data[0]>0) { left=0; right=1; }
+    if (acc_data[0]<0) { left=1; right=0; }
+
+}
+
+/* Control_Motor -------------------------------------------------------------*/
+
+void Control_Motor(void)
+{
+
+    //printf("Diff: %d lux/mems\n\r", diff);
+    motor->SetMaxSpeed(diff);
+
+    if (diff>TOLLERANCE) {
+        if (diff <=RANGE_1)
+        {
+            if (left)  { strcpy(DisplayStr,"E___"); }
+            if (right) { strcpy(DisplayStr,"___3"); }
+        }
+        else if (diff >RANGE_1 & diff <=RANGE_2)
+        {
+            if (left)  { strcpy(DisplayStr,"E==="); }
+            if (right) { strcpy(DisplayStr,"===3"); }
+        }
+        else if (diff >RANGE_2)
+        {
+            if (left)  { strcpy(DisplayStr,"E~~~"); }
+            if (right) { strcpy(DisplayStr,"~~~3"); }
+        }
+
+        // In Case of Change Direction
+        if (left & dir==2)  { changedir=1; }
+        if (right & dir==1) { changedir=1; }
 
-//AnalogIn analog_read(A1); // A1 Conflict with BLE SPI_CS --> Changed in A????
+        // Run only if Stop or Change Direction
+        if (diff>TOLLERANCE & (dir==0 | changedir==1)) {
+            if (left)   { motor->Run(StepperMotor::FWD); dir=1; changedir=0; }
+            if (right)  { motor->Run(StepperMotor::BWD); dir=2; changedir=0; }
+            printf("RUN\n\r");
+        }
+    }
+
+    // Get Motor Position and Control Rotation Block
+    pos = motor->GetPosition();
+    if (pos>STOP | pos<-STOP) {
+        if (pos>0) { motor->GoTo(STOP); }
+        if (pos<0) { motor->GoTo(-STOP); }
+        printf("GOTO\n\r");
+    }
 
+    // Stop Motor
+    if (diff<=TOLLERANCE) { 
+        arrest=1; 
+        if (Display==0) { strcpy(DisplayStr,"----"); }
+        if (Display==2) { strcpy(DisplayStr,"E  3"); }
+    }
+        
+    if (arrest==1 && dir!=0 ) { // Stop just one time
+        motor->HardStop();
+        dir=0;
+        changedir=0;
+        arrest=0;
+        printf("STOP\n\r");
+    }
+
+}
+
+/* Measure_SolarPanel --------------------------------------------------------*/
+
+void Measure_SolarPanel(void)
+{
+    
+    // AnalogIn: 0V return 0.0 , 3.3V return 1.0
+    float measure = 0; //analog_read.read() * 3300;
+    //printf("Measure = %.0f mV\r\n", measure);
+    //board->display->DisplayDigit("A", 0);
+
+    if (Display==1) { sprintf(DisplayStr, "%.0f", measure); }
+
+    board->display->DisplayString(DisplayStr, 4);
+    //printf("%s\n\r", DisplayStr);
+    
+}
 
 /* Bluetooth CallBack ---------------------------------------------------------*/
 
@@ -140,7 +328,13 @@
 
 static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
 {
+    printf ("myonDataWriteCallback attr_handle: %x  att_data[3]: %x  data_length: %d\n\r", eventDataP->handle, eventDataP->data[3], eventDataP->len );
     
+    if (p_customcontrolservice->isContHandle(eventDataP->handle)) // Only if write come from this Service
+    {
+        uint8_t Command = eventDataP->data[3];  // Riceve 8 byte: data[0] + data[1] + data[2] + data[3]
+        printf("myonDataWriteCallback (Command %x)\r\n", Command);
+    }
 }
 
 static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
@@ -154,15 +348,11 @@
     p_BLEdev->gap().startAdvertising();
 }
 
-/* Main ----------------------------------------------------------------------*/
+/* Bluetooth Initialization ---------------------------------------------------*/
 
-int main()
+bool BLE_Initialization(void)
 {
 
-    // Printing to the console
-    printf("SunTracker by Fabio Brembilla\r\n\n");
-    
-    // BLE Initialization
     p_BLEdev = new BLE;
     if (!p_BLEdev) { printf("\r\nBLE Device creation failed\r\n"); }
     const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC};        
@@ -170,91 +360,81 @@
     
     p_BLEdev->init();
 
-    // BLE CallBack Functions
+    // Set BLE CallBack Functions
     p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
     p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
     p_BLEdev->gattServer().onDataRead(onDataReadCallback);
     p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback);
     p_BLEdev->gap().onConnection(onConnectionCallback);
     p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
+    //p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);    
+    //p_BLEdev->gattServer().onDataSent(onDataSentCallback);    
+    //p_BLEdev->gap().onTimeout(onTimeoutCallback);
 
     // BLE Services
     p_customcontrolservice =  new CustomControlService(*p_BLEdev);   
     p_customsensorservice = new CustomSensorService(*p_BLEdev);
 
-    // BLE Advertising
+    // Setup BLE Advertising
     const static char DEVICE_NAME[] = BLE_DEV_NAME;
     p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
-    uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x00,0x00};  
+    uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x00,0x00};    
     p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);    
     p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
     p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
     p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
     p_BLEdev->gap().startAdvertising();
-      
-    printf("BLE_Initialization OK (Line %d)\r\n", __LINE__);
 
-    // Initializing SPI bus
-    DevSPI dev_spi(D11, D12, D13);
-    
-    // Initializing I2C bus
-    DevI2C dev_i2c(D14, D15);  
-    
-    // Initializing Babybear Component 6180XA1
-    static X_NUCLEO_6180XA1 *board;
-    MeasureData_t data_sensor_left, data_sensor_right;
+    return true;
     
-    // Initializing Babybear Component
-    //board=X_NUCLEO_6180XA1::Instance(&dev_i2c); // Con questo comando non si blocca quando Connetto il BLE, ma comunque non fuonziona
-    board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
-    //status=board->InitBoard();
-    if(status) VL6180x_ErrLog("Failed to init the board!\n\r");
+}
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+
+    // Printing to the console
+    printf("SunTracker by Fabio Brembilla\r\n\n");
 
-    // Put GPIO not used as Interrupt in Hi-Z
-    status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); No Present
-    status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    Initialization();
+    printf("Initialization OK (Line %d)\r\n", __LINE__);    
     
-    // Set Babybears
-    status_l=board->sensor_left->AlsSetAnalogueGain(3);
-    status_r=board->sensor_right->AlsSetAnalogueGain(3);
-    status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
-    status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
+    BLE_Initialization();
+    printf("BLE_Initialization OK (Line %d)\r\n", __LINE__);  
+        
+    mybutton.fall(&User_Button_Pressed);
+    printf("Wait Push Button\r\n");
 
-    printf("Initialization OK (Line %d)\r\n", __LINE__);
+    // Loop until push User Button to Set 0 Point
+    strcpy(DisplayStr,"pusH");
+    while(start<1) {
+        board->display->DisplayString(DisplayStr, 4);
+        p_BLEdev->waitForEvent();
+        //printf("%s\n\r", DisplayStr);
+    }
 
     printf("Start Main Loop\r\n");
 
     static int INTLOOP=0;
 
     // Main Loop
-    while(true) {
-        
-        status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left);
-        status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right);
-    
-        babybear = data_sensor_right.lux - data_sensor_left.lux;
-    
-        diff = abs(babybear);
+    while(true)
+    {
+        if (Display==0 | Display==1)    { Measure_Babybear(); }
+        if (Display==2)                 { Measure_Accelerometer(); }
+
+        Control_Motor();
+        Measure_SolarPanel();
         
         INTLOOP++;
-        if (INTLOOP==100) { p_customsensorservice->sendEnvPosition(rand(),0); INTLOOP=0; }
-          
-        //printf("babybear %d\r\n", diff);
+        if (INTLOOP==100) { p_customsensorservice->sendEnvPosition(rand(),0); printf("Read BLE\n\r"); INTLOOP=0; }
         
-        sprintf(DisplayStr, "%d", diff);
-        board->display->DisplayString(DisplayStr, 4);
-            
-        //strcpy(DisplayStr,"pusH");
-        //board->display->DisplayString(DisplayStr, 4);
-            
         p_BLEdev->waitForEvent();
-        }
-}
+    }
 
-// Configuro prima BLE e poi Babybear funziona, ma quando mi collego con il BLE l'app va in crash con il commento.
-// “mbed assertation failed: (hz > 0) && (hz <= 400000), file: C:\Code\git_repo\github\mbed-official\libraries\mbed\targets\hal\TARGET_STM\TARGET_STM32F4\i2c_api.c, line 119”
+    //status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
+    //status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
 
-// Configuro prima Babybear e poi BLE non funziona.
+}

--- a/main_GOOD.cpp.h	Wed Feb 10 12:52:55 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,452 +0,0 @@
-/**
- ******************************************************************************
- * @file    main.cpp
- * @author  Fabio Brembilla
- * @version V2.0.0
- * @date    February, 2016
- * @brief   SunTracker + RemoteControl Vertical Application
- *          This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards
- ******************************************************************************
- * @attention
- *
- * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *   1. Redistributions of source code must retain the above copyright notice,
- *      this list of conditions and the following disclaimer.
- *   2. Redistributions in binary form must reproduce the above copyright notice,
- *      this list of conditions and the following disclaimer in the documentation
- *      and/or other materials provided with the distribution.
- *   3. Neither the name of STMicroelectronics nor the names of its contributors
- *      may be used to endorse or promote products derived from this software
- *      without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- ******************************************************************************
- */
-
-/* Includes ------------------------------------------------------------------*/
-
-// Mbed specific header files
-#include "mbed.h"
-
-// Helper header files
-#include "DevSPI.h"
-#include "DevI2C.h"
-
-// Component specific header files
-#include "l6474_class.h"
-#include "x_nucleo_6180xa1.h"
-#include "x_nucleo_iks01a1.h"
-
-// C header files
-#include <string.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <assert.h>
-
-/* BlueTooth -----------------------------------------------------------------*/
-
-#include "debug.h" // Need for PRINTF
-#include "Utils.h" // Need for STORE_LE_16 and _32
-
-typedef struct {
-    int32_t AXIS_X;
-    int32_t AXIS_Y;
-    int32_t AXIS_Z;
-} AxesRaw_TypeDef;
-
-typedef enum ConnectionStatus_t {
-    DISCONNECTED    =0,
-    CONNECTED       =1
-}cns_t;
-
-const unsigned   LENGTH_OF_LONG_UUID = 16;
-typedef uint16_t ShortUUIDBytes_t;
-typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
-
-#include "CustomControlService.h"
-#include "CustomSensorsService.h"
-
-static BLE *p_BLEdev = NULL;
-
-#define BLE_DEV_NAME "SunTracker"
-#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF
-#define BLE_ADVERTISING_INTERVAL 1000
-
-/* Definitions ---------------------------------------------------------------*/
-
-#define SET_ACC 400     // Set Motor Acceleration
-#define SET_DEC 400     // Set Motor Deceleration
-#define SET_MAX 200     // Set Motor MaxSpeed
-#define SET_MIN 100     // Set Motor MinSpeed
-#define STOP 1000       // Set Motor Stop Position
-#define TOLLERANCE 100  // Tollerance between Left and Right before Start Movement
-#define RANGE_1 200     // Range 1 for Motor Speed
-#define RANGE_2 500     // Range 2 for Motor Speed
-
-/* Variables -----------------------------------------------------------------*/
-
-int16_t dir=0;          // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
-int16_t changedir=0;    // Change Direction: 0 = No, 1 = Yes
-int16_t babybear=0;     // Difference (in Lux) between Left and Right
-int acc_data[3];        // Difference of Accelerometer
-int16_t diff=0;         // Abs of Babybear or Accelerometer difference
-int16_t left=0;         // Left Command for Rotate Direction
-int16_t right=0;        // Right Command for Rotate Direction
-int16_t start=0;        // Waiting User Button Push
-int32_t pos=0;          // Motor Position
-char DisplayStr[5];     // Status Display
-int16_t Display=0;      // Shown on Display: 0 = Motor Speed, 1 = Solar Panel Value, 2 = Manual Control
-int16_t status, status_t, status_b, status_l, status_r; // Babybear Status
-
-/* ---------------------------------------------------------------------------*/
-
-// Initializing SPI bus
-DevSPI dev_spi(D11, D12, D13);
-
-// Initializing I2C bus
-DevI2C dev_i2c(D14, D15);  
-
-// Initializing Motor Component IHM01A1
-static L6474 *motor;
-
-// Initializing Babybear Component 6180XA1
-//static X_NUCLEO_6180XA1 *board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
-//MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
-static X_NUCLEO_6180XA1 *board;
-MeasureData_t data_sensor_top, data_sensor_bottom, data_sensor_left, data_sensor_right;
-    
-// Initializing MEMS Component IKS01A1
-//static X_NUCLEO_IKS01A1 *mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
-//MotionSensor *accelerometer = mems->GetAccelerometer();
-static X_NUCLEO_IKS01A1 *mems;
-MotionSensor *accelerometer;
-
-void DISP_ExecLoopBody(void) {};
-
-//AnalogIn analog_read(A1); // A1 Conflict with BLE SPI_CS --> Changed in A????
-
-InterruptIn mybutton(USER_BUTTON);
-
-/* User_Button_Pressed -------------------------------------------------------*/
-
-void User_Button_Pressed()
-{
-
-    if (start>0) {
-        Display++;
-    }
-    if (Display>2) {
-        Display=0;
-    }
-    if (start==0) {
-        start=1;
-    }
-    
-    printf("PUSH Display %d Start %d\r\n", Display, start);
-
-}
-
-/* Initialization ------------------------------------------------------------*/
-
-bool Initialization(void)
-{
-  
-    // Initializing MEMS Component
-    mems=X_NUCLEO_IKS01A1::Instance(&dev_i2c);
-    accelerometer = mems->GetAccelerometer();
-   
-//----
-
-    // Initializing Babybear Component
-    board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
-
-/* FABIO
-    status=board->InitBoard();
-    if(status) VL6180x_ErrLog("Failed to init the board!\n\r");
-*/
-
-    // Put GPIO not used as Interrupt in Hi-Z
-    status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); No Present
-    status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
-    
-    // Set Babybears
-    status_l=board->sensor_left->AlsSetAnalogueGain(3);
-    status_r=board->sensor_right->AlsSetAnalogueGain(3);
-    status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
-    status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
-
-//----
-
-    // Initializing Motor Component
-    motor = new L6474(D2, D8, D7, D9, D10, dev_spi);
-    if (motor->Init(NULL) != COMPONENT_OK)
-        return false;
-
-    motor->SetStepMode(STEP_MODE_1_8);  // Default is STEP_MODE_1_16
-
-    // Set defaults Motor Speed
-    motor->SetAcceleration(SET_ACC);
-    motor->SetDeceleration(SET_DEC);
-    motor->SetMaxSpeed(SET_MAX);        // Variable by Light/Mems Sensors
-    motor->SetMinSpeed(SET_MIN);
-
-    return true;
-
-}
-
-/* Measure_Babybear ----------------------------------------------------------*/
-
-void Measure_Babybear(void)
-{
-
-    status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left);
-    status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right);
-
-    babybear = data_sensor_right.lux - data_sensor_left.lux;
-
-    diff = abs(babybear);
-
-    if (babybear>0) {
-        left=0;
-        right=1;
-    }
-    if (babybear<0) {
-        left=1;
-        right=0;
-    }
-
-}
-
-/* Measure_Accelerometer -----------------------------------------------------*/
-
-void Measure_Accelerometer(void)
-{
-
-    accelerometer->Get_X_Axes(acc_data);
-
-    diff = abs(acc_data[0]);
-
-    if (acc_data[0]>0) {
-        left=0;
-        right=1;
-    }
-    if (acc_data[0]<0) {
-        left=1;
-        right=0;
-    }
-
-}
-
-/* Control_Motor -------------------------------------------------------------*/
-
-void Control_Motor(void)
-{
-
-    //printf("Diff: %d lux/mems\n\r", diff);
-    motor->SetMaxSpeed(diff);
-
-    if (diff>TOLLERANCE) {
-        if (diff <=RANGE_1)
-        {
-            if (left)  { strcpy(DisplayStr,"E___"); }
-            if (right) { strcpy(DisplayStr,"___3"); }
-        }
-        else if (diff >RANGE_1 & diff <=RANGE_2)
-        {
-            if (left)  { strcpy(DisplayStr,"E==="); }
-            if (right) { strcpy(DisplayStr,"===3"); }
-        }
-        else if (diff >RANGE_2)
-        {
-            if (left)  { strcpy(DisplayStr,"E~~~"); }
-            if (right) { strcpy(DisplayStr,"~~~3"); }
-        }
-
-        // In Case of Change Direction
-        if (left & dir==2)  { changedir=1; }
-        if (right & dir==1) { changedir=1; }
-
-        // Run only if Stop or Change Direction
-        if (diff>TOLLERANCE & (dir==0 | changedir==1)) {
-            if (left)   { motor->Run(StepperMotor::FWD); dir=1; changedir=0; }
-            if (right)  { motor->Run(StepperMotor::BWD); dir=2; changedir=0; }
-        }
-    }
-
-    // Get Motor Position and Control Rotation Block
-    pos = motor->GetPosition();
-    if (pos>STOP | pos<-STOP) {
-        if (pos>0) { motor->GoTo(STOP); }
-        if (pos<0) { motor->GoTo(-STOP); }
-        printf("GOTO\n\r"); // Without this command, the motor remain in stop
-    }
-
-    // Stop Motor
-    if (diff<=TOLLERANCE) { // It continues to send the command to stop the motor. Think to do it just one time
-        motor->HardStop();
-        if (Display==0) { strcpy(DisplayStr,"----"); }
-        if (Display==2) { strcpy(DisplayStr,"E  3"); }
-        dir=0;
-        changedir=0;
-        //printf("STOP\n\r");
-    }
-
-}
-
-/* Measure_SolarPanel --------------------------------------------------------*/
-
-void Measure_SolarPanel(void)
-{
-
-    // AnalogIn: 0V return 0.0 , 3.3V return 1.0
-//    float measure = analog_read.read() * 3300;
-    float measure = 0;
-    //printf("Measure = %.0f mV\r\n", measure);
-    //board->display->DisplayDigit("A", 0);
-
-    if (Display==1) {
-        sprintf(DisplayStr, "%.0f", measure);
-    }
-
-    board->display->DisplayString(DisplayStr, 4);
-    printf("%s\n\r", DisplayStr);
-
-}
-
-/* Bluetooth CallBack ---------------------------------------------------------*/
-
-static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
-{
-    
-}
-
-static void onUpdatesDisabledCallback(Gap::Handle_t handle)
-{
-    
-}
-
-static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
-{
-    
-}
-
-static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
-{
-    
-}
-
-static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
-{
-    
-}
-
-static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
-{
-    
-}
-
-/* Bluetooth Initialization ---------------------------------------------------*/
-
-void BLE_Initialization(void)
-{
-
-    p_BLEdev = new BLE;
-    if (!p_BLEdev) { printf("\r\nBLE Device creation failed\r\n"); }
-    const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC};        
-    p_BLEdev->gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE);
-    
-    p_BLEdev->init();
-
-    // Set BLE CallBack Functions
-    p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
-    p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
-    p_BLEdev->gattServer().onDataRead(onDataReadCallback);
-    p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback);
-    p_BLEdev->gap().onConnection(onConnectionCallback);
-    p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
-    //p_BLEdev->gattServer().onConfirmationReceived(onConfirmationReceivedCallback);    
-    //p_BLEdev->gattServer().onDataSent(onDataSentCallback);    
-    //p_BLEdev->gap().onTimeout(onTimeoutCallback);
-
-    // Setup BLE Advertising
-    const static char DEVICE_NAME[] = BLE_DEV_NAME;
-    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
-    #ifdef USE_SENSOR_FUSION_LIB        
-    uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x01,0x80};
-    #else
-    uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x00,0x00};
-    #endif      
-    p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);    
-    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
-    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
-    p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
-    p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
-    p_BLEdev->gap().startAdvertising();
-
-}
-
-/* Main ----------------------------------------------------------------------*/
-
-int main()
-{
-
-    // Printing to the console
-    printf("SunTracker by Fabio Brembilla\r\n\n");
-
-    Initialization();
-
-    printf("Initialization OK (Line %d)\r\n", __LINE__);    
-    
-    BLE_Initialization();
-        
-    printf("BLE_Initialization OK (Line %d)\r\n", __LINE__);
-        
-    mybutton.fall(&User_Button_Pressed);
-    
-    printf("Main Initializations OK (Line %d)\r\n", __LINE__);
-    printf("Wait Push Button\r\n");
-
-    // Loop until push User Button to Set 0 Point
-    strcpy(DisplayStr,"pusH");
-    while(start<1) {
-        board->display->DisplayString(DisplayStr, 4);
-        printf("%s\n\r", DisplayStr);
-        //wait(0.1); // Need one command otherwise remain always in loop
-    }
-
-    printf("Start Main Loop\r\n");
-
-    // Main Loop
-    while(true) {
-        if (Display==0 | Display==1)    {
-            Measure_Babybear();
-        }
-        if (Display==2)                 {
-            Measure_Accelerometer();
-        }
-
-        Control_Motor();
-        Measure_SolarPanel();
-        
-        p_BLEdev->waitForEvent();
-    }
-
-    //status_l=board->sensor_left->StopMeasurement(als_continuous_polling);
-    //status_r=board->sensor_right->StopMeasurement(als_continuous_polling);
-
-}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_only_BLE_BB.cpp.h	Fri Feb 12 10:52:50 2016 +0000
@@ -0,0 +1,267 @@
+/**
+ ******************************************************************************
+ * @file    main.cpp
+ * @author  Fabio Brembilla
+ * @version V2.0.0
+ * @date    February, 2016
+ * @brief   SunTracker + RemoteControl Vertical Application
+ *          This application use IHM01A1, 6180XA1, IKS01A1, IDB0XA1 expansion boards
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *      without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+ */
+
+/* Includes ------------------------------------------------------------------*/
+
+// Mbed specific header files
+#include "mbed.h"
+
+// Helper header files
+#include "DevSPI.h"
+#include "DevI2C.h"
+
+// Component specific header files
+#include "l6474_class.h"
+#include "x_nucleo_6180xa1.h"
+#include "x_nucleo_iks01a1.h"
+
+// C header files
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+
+/* BlueTooth -----------------------------------------------------------------*/
+
+#include "debug.h" // Need for PRINTF
+#include "Utils.h" // Need for STORE_LE_16 and _32
+
+typedef struct {
+    int32_t AXIS_X;
+    int32_t AXIS_Y;
+    int32_t AXIS_Z;
+} AxesRaw_TypeDef;
+
+typedef enum ConnectionStatus_t {
+    DISCONNECTED    =0,
+    CONNECTED       =1
+}cns_t;
+
+const unsigned   LENGTH_OF_LONG_UUID = 16;
+typedef uint16_t ShortUUIDBytes_t;
+typedef uint8_t  LongUUIDBytes_t[LENGTH_OF_LONG_UUID];
+
+#include "CustomControlService.h"
+#include "CustomSensorsService.h"
+
+static BLE *p_BLEdev = NULL;
+
+#define BLE_DEV_NAME "SunTracker"
+#define BLE_DEV_MAC 0xAA,0xBB,0xCC,0xDD,0xEE,0xFF
+#define BLE_ADVERTISING_INTERVAL 1000
+
+static CustomControlService *p_customcontrolservice = NULL;
+static CustomSensorService  *p_customsensorservice = NULL;
+
+/* Definitions ---------------------------------------------------------------*/
+
+#define SET_ACC 400     // Set Motor Acceleration
+#define SET_DEC 400     // Set Motor Deceleration
+#define SET_MAX 200     // Set Motor MaxSpeed
+#define SET_MIN 100     // Set Motor MinSpeed
+#define STOP 1000       // Set Motor Stop Position
+#define TOLLERANCE 100  // Tollerance between Left and Right before Start Movement
+#define RANGE_1 200     // Range 1 for Motor Speed
+#define RANGE_2 500     // Range 2 for Motor Speed
+
+/* Variables -----------------------------------------------------------------*/
+
+int16_t dir=0;          // Motor Rotation Direction: 0 = Stop, 1 = Anticlockwise, 2 = Clockwise
+int16_t changedir=0;    // Change Direction: 0 = No, 1 = Yes
+int16_t babybear=0;     // Difference (in Lux) between Left and Right
+int acc_data[3];        // Difference of Accelerometer
+int16_t diff=0;         // Abs of Babybear or Accelerometer difference
+int16_t left=0;         // Left Command for Rotate Direction
+int16_t right=0;        // Right Command for Rotate Direction
+int16_t start=0;        // Waiting User Button Push
+int32_t pos=0;          // Motor Position
+char DisplayStr[5];     // Status Display
+int16_t Display=0;      // Shown on Display: 0 = Motor Speed, 1 = Solar Panel Value, 2 = Manual Control
+int16_t status, status_t, status_b, status_l, status_r; // Babybear Status
+
+/* ---------------------------------------------------------------------------*/  
+
+//void DISP_ExecLoopBody(void) {};
+
+//AnalogIn analog_read(A1); // A1 Conflict with BLE SPI_CS --> Changed in A????
+
+
+/* Bluetooth CallBack ---------------------------------------------------------*/
+
+static void onUpdatesEnabledCallback(GattAttribute::Handle_t handle)
+{
+    
+}
+
+static void onUpdatesDisabledCallback(Gap::Handle_t handle)
+{
+    
+}
+
+static void onDataReadCallback(const GattReadCallbackParams *eventDataP)
+{
+    
+}
+
+static void myonDataWriteCallback(const GattWriteCallbackParams *eventDataP)
+{
+    printf ("myonDataWriteCallback attr_handle: %x  att_data[3]: %x  data_length: %d\n\r", eventDataP->handle, eventDataP->data[3], eventDataP->len );
+    
+    if (p_customcontrolservice->isContHandle(eventDataP->handle)) // Only if write come from this Service
+    {
+        uint8_t Command = eventDataP->data[3];  // Riceve 8 byte: data[0] + data[1] + data[2] + data[3]
+        printf("myonDataWriteCallback (Command %x)\r\n", Command);
+    } 
+
+}
+
+static void onConnectionCallback(const Gap::ConnectionCallbackParams_t * connectionParams)
+{
+    printf("onConnectionCallback (Line %d)\r\n", __LINE__);   
+}
+
+static void onDisconnectionCallback(const Gap::DisconnectionCallbackParams_t * disConnectionReason)
+{
+    printf("onDisconnectionCallback (Line %d)\r\n", __LINE__);
+    p_BLEdev->gap().startAdvertising();
+}
+
+/* Main ----------------------------------------------------------------------*/
+
+int main()
+{
+
+    // Printing to the console
+    printf("SunTracker by Fabio Brembilla\r\n\n");
+    
+    // BLE Initialization
+    p_BLEdev = new BLE;
+    if (!p_BLEdev) { printf("\r\nBLE Device creation failed\r\n"); }
+    const Gap::Address_t BLE_address_BE = {BLE_DEV_MAC};        
+    p_BLEdev->gap().setAddress(BLEProtocol::AddressType::PUBLIC, BLE_address_BE);
+    
+    p_BLEdev->init();
+
+    // BLE CallBack Functions
+    p_BLEdev->gattServer().onUpdatesEnabled(onUpdatesEnabledCallback);
+    p_BLEdev->gattServer().onUpdatesDisabled(onUpdatesDisabledCallback);
+    p_BLEdev->gattServer().onDataRead(onDataReadCallback);
+    p_BLEdev->gattServer().onDataWritten(myonDataWriteCallback);
+    p_BLEdev->gap().onConnection(onConnectionCallback);
+    p_BLEdev->gap().onDisconnection(onDisconnectionCallback);
+
+    // BLE Services
+    p_customcontrolservice =  new CustomControlService(*p_BLEdev);   
+    p_customsensorservice = new CustomSensorService(*p_BLEdev);
+
+    // BLE Advertising
+    const static char DEVICE_NAME[] = BLE_DEV_NAME;
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    uint8_t dat[] = {0x01,0x80,0x00,0xFC,0x00,0x00};  
+    p_BLEdev->gap().accumulateScanResponse(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,dat,6);    
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
+    p_BLEdev->gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    p_BLEdev->gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    p_BLEdev->gap().setAdvertisingInterval(BLE_ADVERTISING_INTERVAL);
+    p_BLEdev->gap().startAdvertising();
+      
+    printf("BLE_Initialization OK (Line %d)\r\n", __LINE__);
+
+    // Initializing SPI bus
+    DevSPI dev_spi(D11, D12, D13);
+    
+    // Initializing I2C bus
+    DevI2C dev_i2c(D14, D15);  
+    
+    // Initializing Babybear Component 6180XA1
+    static X_NUCLEO_6180XA1 *board;
+    MeasureData_t data_sensor_left, data_sensor_right;
+    
+    // Initializing Babybear Component
+    //board=X_NUCLEO_6180XA1::Instance(&dev_i2c); // Con questo comando non si blocca quando Connetto il BLE, ma comunque non fuonziona
+    board=X_NUCLEO_6180XA1::Instance(&dev_i2c, NC, NC, NC, NC);
+    //status=board->InitBoard();
+    if(status) VL6180x_ErrLog("Failed to init the board!\n\r");
+
+    // Put GPIO not used as Interrupt in Hi-Z
+    status_t=board->sensor_top->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    //status_b=board->sensor_botton->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF); No Present
+    status_l=board->sensor_left->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    status_r=board->sensor_right->SetGPIOxFunctionality(1, GPIOx_SELECT_OFF);
+    
+    // Set Babybears
+    status_l=board->sensor_left->AlsSetAnalogueGain(3);
+    status_r=board->sensor_right->AlsSetAnalogueGain(3);
+    status_l=board->sensor_left->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
+    status_r=board->sensor_right->StartMeasurement(als_continuous_polling, NULL, NULL, NULL);
+
+    printf("Initialization OK (Line %d)\r\n", __LINE__);
+
+    printf("Start Main Loop\r\n");
+
+    static int INTLOOP=0;
+
+    // Main Loop
+    while(true) {
+        
+        status_l=board->sensor_left->GetMeasurement(als_continuous_polling, &data_sensor_left);
+        status_r=board->sensor_right->GetMeasurement(als_continuous_polling, &data_sensor_right);
+    
+        babybear = data_sensor_right.lux - data_sensor_left.lux;
+    
+        diff = abs(babybear);
+        
+        INTLOOP++;
+        if (INTLOOP==100) { p_customsensorservice->sendEnvPosition(rand(),0); INTLOOP=0; }
+          
+        //printf("babybear %d\r\n", diff);
+        
+        sprintf(DisplayStr, "%d", diff);
+        board->display->DisplayString(DisplayStr, 4);
+            
+        //strcpy(DisplayStr,"pusH");
+        //board->display->DisplayString(DisplayStr, 4);
+            
+        p_BLEdev->waitForEvent();
+        }
+}
+
+// Configuro prima BLE e poi Babybear funziona, ma quando mi collego con il BLE l'app va in crash con il commento.
+// “mbed assertation failed: (hz > 0) && (hz <= 400000), file: C:\Code\git_repo\github\mbed-official\libraries\mbed\targets\hal\TARGET_STM\TARGET_STM32F4\i2c_api.c, line 119”
+
+// Configuro prima Babybear e poi BLE non funziona.