Diff: main.cpp

Revision:

5:92a861d48253

Parent:

4:360dc34c3769

Child:

6:4bc839979c55

Child:

8:24ebe29a65bd

--- a/main.cpp	Fri May 07 14:41:08 2021 +0000
+++ b/main.cpp	Fri May 07 14:46:18 2021 +0000
@@ -8,14 +8,14 @@
  *
  * The Reset button can be used to restart the program.
  *
- * *** Note : 
+ * *** Note :
  * Default Mbed build system settings disable print floating-point support.
  * Offline builds can enable this, again.
  * https://github.com/ARMmbed/mbed-os/blob/master/platform/source/minimal-printf/README.md
  * .\mbed-os\platform\mbed_lib.json
  *
  */
- 
+
 #include <stdio.h>
 #include <time.h>
 
@@ -25,17 +25,17 @@
 #include "vl53L3_I2c.h"
 
 
-#define I2C_SDA   D14 
-#define I2C_SCL   D15 
+#define I2C_SDA   D14
+#define I2C_SCL   D15
 
-#define MEASUREMENTTIMING  55 
+#define MEASUREMENTTIMING  55
 
 static XNucleo53L3A2 *board=NULL;
 #if (MBED_VERSION  > 60300)
-    UnbufferedSerial  pc(USBTX, USBRX);
-    extern "C" void wait_ms(int ms);
+UnbufferedSerial  pc(USBTX, USBRX);
+extern "C" void wait_ms(int ms);
 #else
-    Serial pc(SERIAL_TX, SERIAL_RX);
+Serial pc(SERIAL_TX, SERIAL_RX);
 #endif
 
 static int int_centre_result = 0;
@@ -43,8 +43,9 @@
 static int int_right_result = 0;
 
 
-class WaitForMeasurement {
-    
+class WaitForMeasurement
+{
+
 public:
 
 
@@ -52,25 +53,25 @@
 // There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things.
 // We keep things simple by only raising a flag so all the real work is done outside the interrupt.
 // This is designed around MBED V2 which doesn't have the RTOS features that would make this work nicely e.g. semaphores/queues.
-WaitForMeasurement(): _interrupt(A1)
-{
-}
+    WaitForMeasurement(): _interrupt(A1)
+    {
+    }
 
 
     // constructor - Sensor is not used and can be removed
     WaitForMeasurement(PinName pin,VL53LX_DEV Dev) : _interrupt(pin)          // create the InterruptIn on the pin specified to Counter
     {
-         Devlocal = Dev;
+        Devlocal = Dev;
         _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach increment function of this counter instance
-        
+
     }
-    
+
     void process_right_interrupt()
     {
-          printf("processing right interrupt\n");
+        printf("processing right interrupt\n");
     }
 
-  // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
+    // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
     void got_interrupt()
     {
         _count++;
@@ -83,7 +84,7 @@
             int_right_result = 1;  //flag to main that interrupt happened
     }
 
-    
+
     //destructor
     ~WaitForMeasurement()
     {
@@ -95,7 +96,7 @@
     InterruptIn _interrupt;
     volatile int _count;
     VL53LX_DEV Devlocal;
-    int status;    
+    int status;
 };
 
 
@@ -108,17 +109,17 @@
 
 
 
- 
+
 /*=================================== Main ==================================
 =============================================================================*/
 int main()
-{   
+{
     int status;
     VL53L3 * Sensor;
     uint16_t wordData;
     uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
-  
-    
+
+
     WaitForMeasurement* int2;
     WaitForMeasurement* int1;
     WaitForMeasurement* int3;
@@ -139,13 +140,13 @@
         printf("Failed to init board!\r\n");
         return 0;
     }
-       
-        
+
+
     printf("board initiated! - %d\r\n", status);
-                                                
-        for (ToFSensor=0;ToFSensor<3;ToFSensor++){
+
+    for (ToFSensor=0; ToFSensor<3; ToFSensor++) {
         wait_ms(15);
-        switch(ToFSensor){
+        switch(ToFSensor) {
             case 0:
                 if (board->sensor_centre== NULL ) continue;
                 Dev=&devCentre;
@@ -155,22 +156,22 @@
                 break;
             case 1:
                 if (board->sensor_left== NULL ) continue;
-                Dev=&devLeft; 
+                Dev=&devLeft;
                 Sensor=board->sensor_left;
                 Dev->I2cDevAddr = NEW_SENSOR_LEFT_ADDRESS;
                 printf("configuring left channel \n");
                 break;
             case 2:
                 if (board->sensor_right== NULL ) continue;
-                Dev=&devRight;  
+                Dev=&devRight;
                 Sensor=board->sensor_right;
                 Dev->I2cDevAddr = NEW_SENSOR_RIGHT_ADDRESS;
                 printf("configuring right channel \n");
-                break;      
+                break;
             default:
-               printf(" error in switch, invalid ToF sensor \n");
+                printf(" error in switch, invalid ToF sensor \n");
         }
-        
+
 // configure the sensors
         Dev->comms_speed_khz = 400;
 
@@ -178,148 +179,133 @@
 
         Sensor->VL53LX_RdWord(Dev, 0x01, &wordData);
         printf("VL53L1X: %02X   %d\n\r", wordData,Dev->I2cDevAddr);
-/* Device Initialization and setting */  
+        /* Device Initialization and setting */
 
         status = Sensor->VL53LX_DataInit();
         status = Sensor->VL53LX_SetDistanceMode(VL53LX_DISTANCEMODE_LONG);
-        status = Sensor->VL53LX_SetMeasurementTimingBudgetMicroSeconds( MEASUREMENTTIMING * 1000);                      
+        status = Sensor->VL53LX_SetMeasurementTimingBudgetMicroSeconds( MEASUREMENTTIMING * 1000);
         status = Sensor->VL53LX_SmudgeCorrectionEnable(VL53LX_SMUDGE_CORRECTION_SINGLE);
         status = Sensor->VL53LX_SetXTalkCompensationEnable(1);
 
-        
-        }
-        
-        if (board->sensor_centre!= NULL )
-        {
-            printf("starting interrupt centre\n");
-            Sensor=board->sensor_centre;
-            devCentre.I2cDevAddr = NEW_SENSOR_CENTRE_ADDRESS;
-            int1 =  new WaitForMeasurement(A2,&devCentre);
-            status = Sensor->VL53LX_StartMeasurement();
-            printf("VL53L1_StartMeasurement %d \n",status);
+
+    }
+
+    if (board->sensor_centre!= NULL ) {
+        printf("starting interrupt centre\n");
+        Sensor=board->sensor_centre;
+        devCentre.I2cDevAddr = NEW_SENSOR_CENTRE_ADDRESS;
+        int1 =  new WaitForMeasurement(A2,&devCentre);
+        status = Sensor->VL53LX_StartMeasurement();
+        printf("VL53L1_StartMeasurement %d \n",status);
+        status = board->sensor_centre->VL53LX_ClearInterruptAndStartMeasurement();
+    }
+
+
+    if (board->sensor_left!= NULL ) {
+        printf("starting interrupt left\n");
+        Sensor=board->sensor_left;
+        devLeft.I2cDevAddr = NEW_SENSOR_LEFT_ADDRESS;
+        int2 = new WaitForMeasurement(D8,&devLeft);
+        status = Sensor->VL53LX_StartMeasurement();
+        printf("VL53L1_StartMeasurement %d \n",status);
+        status = board->sensor_left->VL53LX_ClearInterruptAndStartMeasurement();
+    }
+
+    if (board->sensor_right!= NULL ) {
+        printf("starting interrupt right\n");
+        Sensor=board->sensor_right;
+        devRight.I2cDevAddr = NEW_SENSOR_RIGHT_ADDRESS;
+        int3 = new WaitForMeasurement(D2,&devRight);
+        status = Sensor->VL53LX_StartMeasurement();
+        printf("VL53L1_StartMeasurement %d \n",status);
+        status = board->sensor_right->VL53LX_ClearInterruptAndStartMeasurement();
+    }
+
+
+    // loop waiting for interrupts to happen. This is signaled by   int_centre_result,int_left_result or int_right_result
+    // being non zero. The are set back to zero when processing is completed
+    while (1) {
+
+        VL53LX_MultiRangingData_t MultiRangingData;
+        VL53LX_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
+
+        wait_ms(10);
+
+        if (int_centre_result != 0) {
+            int_centre_result = 0;
+            status = board->sensor_centre->VL53LX_GetMultiRangingData( pMultiRangingData);
+            int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
+            if (( no_of_object_found < 10 ) && ( no_of_object_found != 0 )) {
+                for(int j=0; j<no_of_object_found; j++) {
+                    if (pMultiRangingData->RangeData[j].RangeStatus == 0) {
+                        printf("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
+                               pMultiRangingData->RangeData[j].RangeStatus,
+                               pMultiRangingData->RangeData[j].RangeMilliMeter,
+                               (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65535.0),
+                               (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65535.0));
+                    }
+                }
+            }
+            int_centre_result = 0;
+            wait_ms( MEASUREMENTTIMING );
             status = board->sensor_centre->VL53LX_ClearInterruptAndStartMeasurement();
         }
-        
+
 
-        if (board->sensor_left!= NULL )
-        {
-            printf("starting interrupt left\n");
-            Sensor=board->sensor_left;
-            devLeft.I2cDevAddr = NEW_SENSOR_LEFT_ADDRESS;
-            int2 = new WaitForMeasurement(D8,&devLeft);
-            status = Sensor->VL53LX_StartMeasurement();
-            printf("VL53L1_StartMeasurement %d \n",status);
+        if (int_left_result != 0) {
+            int_left_result = 0;
+            status = board->sensor_left->VL53LX_GetMultiRangingData( pMultiRangingData);
+            if ( status == 0) {
+                int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
+                if (( no_of_object_found < 10 ) && ( no_of_object_found != 0 )) {
+                    for(int j=0; j<no_of_object_found; j++) {
+                        if (pMultiRangingData->RangeData[j].RangeStatus == 0) {
+                            printf("left  \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
+                                   pMultiRangingData->RangeData[j].RangeStatus,
+                                   pMultiRangingData->RangeData[j].RangeMilliMeter,
+                                   (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535.0),
+                                   (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535.0) );
+                        }
+                    }
+                }
+            }
+            wait_ms( MEASUREMENTTIMING );
             status = board->sensor_left->VL53LX_ClearInterruptAndStartMeasurement();
         }
 
-        if (board->sensor_right!= NULL )
-        {
-            printf("starting interrupt right\n");
-            Sensor=board->sensor_right;
-            devRight.I2cDevAddr = NEW_SENSOR_RIGHT_ADDRESS;
-            int3 = new WaitForMeasurement(D2,&devRight);
-            status = Sensor->VL53LX_StartMeasurement();
-            printf("VL53L1_StartMeasurement %d \n",status);
-            status = board->sensor_right->VL53LX_ClearInterruptAndStartMeasurement();
-        }
-        
-    
-       // loop waiting for interrupts to happen. This is signaled by   int_centre_result,int_left_result or int_right_result
-       // being non zero. The are set back to zero when processing is completed
-        while (1)
-        {
 
-            VL53LX_MultiRangingData_t MultiRangingData;
-            VL53LX_MultiRangingData_t *pMultiRangingData = &MultiRangingData;   
-            
-            wait_ms(10);
-            
-            if (int_centre_result != 0)
-            {
-                int_centre_result = 0;
-                status = board->sensor_centre->VL53LX_GetMultiRangingData( pMultiRangingData);
+        if (int_right_result != 0) {
+            // clear interrupt flag
+            int_right_result = 0;
+            status = board->sensor_right->VL53LX_GetMultiRangingData( pMultiRangingData);
+            if ( status == 0) {
+                // if valid result print it
                 int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
-                if (( no_of_object_found < 10 ) && ( no_of_object_found != 0 ))
-                {
-                    for(int j=0;j<no_of_object_found;j++){
-                        if (pMultiRangingData->RangeData[j].RangeStatus == 0) 
-                        {
-                            printf("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
-                                pMultiRangingData->RangeData[j].RangeStatus,
-                                pMultiRangingData->RangeData[j].RangeMilliMeter,
-                                (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65535.0),
-                                (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65535.0));
+                if (( no_of_object_found < 10 ) && ( no_of_object_found != 0 )) {
+                    for(int j=0; j<no_of_object_found; j++) {
+                        if (pMultiRangingData->RangeData[j].RangeStatus == 0) {
+                            printf("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
+                                   pMultiRangingData->RangeData[j].RangeStatus,
+                                   pMultiRangingData->RangeData[j].RangeMilliMeter,
+                                   (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65535.0),
+                                   (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65535.0) );
                         }
                     }
                 }
-                int_centre_result = 0;
-                wait_ms( MEASUREMENTTIMING );
-                status = board->sensor_centre->VL53LX_ClearInterruptAndStartMeasurement();
             }
-
+            wait_ms( MEASUREMENTTIMING );
+            // clear theinterrupt and wait for another result
+            status = board->sensor_right->VL53LX_ClearInterruptAndStartMeasurement();
 
-            if (int_left_result != 0)
-            {
-                int_left_result = 0;
-                status = board->sensor_left->VL53LX_GetMultiRangingData( pMultiRangingData);
-                if ( status == 0)
-                {
-                    int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
-                    if (( no_of_object_found < 10 ) && ( no_of_object_found != 0 ))
-                    {
-                        for(int j=0;j<no_of_object_found;j++){
-                            if (pMultiRangingData->RangeData[j].RangeStatus == 0) 
-                            {
-                                printf("left  \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
-                                    pMultiRangingData->RangeData[j].RangeStatus,
-                                    pMultiRangingData->RangeData[j].RangeMilliMeter,
-                                    (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535.0),
-                                    (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535.0) );
-                            }
-                        }
-                    }
-                }
-                wait_ms( MEASUREMENTTIMING );
-                status = board->sensor_left->VL53LX_ClearInterruptAndStartMeasurement();
-            }
-            
-            
-            if (int_right_result != 0)
-            {
-                // clear interrupt flag
-                int_right_result = 0;
-                status = board->sensor_right->VL53LX_GetMultiRangingData( pMultiRangingData);
-                if ( status == 0)
-                {
-                    // if valid result print it
-                    int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
-                    if (( no_of_object_found < 10 ) && ( no_of_object_found != 0 ))
-                    {
-                        for(int j=0;j<no_of_object_found;j++){
-                            if (pMultiRangingData->RangeData[j].RangeStatus == 0) 
-                            {
-                                printf("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
-                                    pMultiRangingData->RangeData[j].RangeStatus,
-                                    pMultiRangingData->RangeData[j].RangeMilliMeter,
-                                    (pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65535.0),
-                                    (pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65535.0) );
-                            }
-                        }
-                    }
-                }
-                wait_ms( MEASUREMENTTIMING );
-                // clear theinterrupt and wait for another result
-                status = board->sensor_right->VL53LX_ClearInterruptAndStartMeasurement();
+        }
+    }
+    printf("terminated");
+}
 
-            }
-        }
-        printf("terminated");
-    }
-  
 #if (MBED_VERSION  > 60300)
 extern "C" void wait_ms(int ms)
- {
+{
     thread_sleep_for(ms);
- }
+}
 #endif
   
\ No newline at end of file