Diff: main.cpp

Revision:

1:b725652b9b42

Parent:

0:231de8959869

Child:

2:306c6c49a327

--- a/main.cpp	Wed Oct 09 08:08:38 2019 +0000
+++ b/main.cpp	Wed Oct 09 10:09:57 2019 +0000
@@ -8,17 +8,8 @@
 #include <std_msgs/String.h>
 #include <std_msgs/Int32.h>
 
-ros::NodeHandle  nh;
-std_msgs::Int32 int_msg;
-std_msgs::String str_msg;
-ros::Publisher chatter("chatter", &str_msg);
-ros::Publisher range_pub("range", &int_msg);
-
-char hello[13] = "hello world!";
-
 DigitalOut led = LED1;
 
-Serial pc(SERIAL_TX, SERIAL_RX); 
 // set-up serial to pc 
 I2C i2c(I2C_SDA, I2C_SCL); 
 // Set up I²C on the STM32 NUCLEO-401RE 
@@ -101,13 +92,15 @@
         WriteByte(0x01a7, 0x1f); 
         WriteByte(0x0030, 0x00);
         
+        /*
         // Recommended : Public registers - See data sheet for more detail 
-        //WriteByte(0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes 
-        //WriteByte(0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time) 
-        //WriteByte(0x003f, 0x46); // Sets the light and dark gain (upper  // nibble). Dark gain should not be       // changed. 
-        //WriteByte(0x0031, 0xFF); // sets the # of range measurements after  // which auto calibration of system is // performed 
-        //WriteByte(0x0041, 0x63); // Set ALS integration time to 100ms
-        //WriteByte(0x002e, 0x01); // perform a single temperature calibration // of the ranging sensor         
+        WriteByte(0x0011, 0x10); // Enables polling for ‘New Sample ready’ when measurement completes 
+        WriteByte(0x010a, 0x30); // Set the averaging sample period (compromise between lower noise and increased execution time) 
+        WriteByte(0x003f, 0x46); // Sets the light and dark gain (upper  // nibble). Dark gain should not be       // changed. 
+        WriteByte(0x0031, 0xFF); // sets the # of range measurements after  // which auto calibration of system is // performed 
+        WriteByte(0x0041, 0x63); // Set ALS integration time to 100ms
+        WriteByte(0x002e, 0x01); // perform a single temperature calibration // of the ranging sensor         
+        */
         
         WriteByte(0x016, 0x00); //change fresh out of set status to 0    
     }    
@@ -132,19 +125,15 @@
     // check the status    
     status = ReadByte(0x04f);            
     range_status = status & 0x07; 
-    
-    printf("status en cours \n");
            
     // wait for new measurement ready status    
-    while (range_status !=  0x00) 
+    while (range_status !=  0x04) 
     {         
-        printf("status = %d\n", status);
         status = ReadByte(0x04f);        
         range_status = status & 0x07;        
         wait_ms(1); // (can be removed)        
     }
     
-    //printf("range polled \n");
     return 0;     
 }
 
@@ -168,42 +157,46 @@
 /////////////////////////////////////////////////////////////////// 
 //  Main Program loop 
 /////////////////////////////////////////////////////////////////// 
-int main() {    
+int main() {   
+ 
+    ros::NodeHandle  nh;
+    nh.initNode();
     
-    nh.initNode();
+    std_msgs::Int32 int_msg;
+    std_msgs::String str_msg;
+    ros::Publisher chatter("chatter", &str_msg);
+    ros::Publisher range_pub("range", &int_msg);
+
+    char hello[13] = "hello world!";
+    
     nh.advertise(chatter);
     nh.advertise(range_pub);
+    
     int range; 
     // load settings onto VL6180X    
     VL6180_Init(); 
-    
-    //char byte;
-    //byte = ReadByte(0x04d);
-    printf("RESULT_RANGE_STATUS %d\n ", byte);
-    
-    //wait(1);
-    
-    // start single range measurement        
-    
-    
+  
     while (1)
     {    
         led = !led;
         str_msg.data = hello;
         chatter.publish( &str_msg );
-        VL6180_Start_Range();  
+        
+        // start range measurement        
+        VL6180_Start_Range();
         // poll the VL6180 till new sample ready        
-        VL6180_Poll_Range();              
+        //VL6180_Poll_Range();              
         // read range result        
         range = VL6180_Read_Range();                         
         // clear the interrupt on VL6180        
         VL6180_Clear_Interrupts();                 
-        // send range to pc by serial 
+        // send range to pc by serial
+         
         int_msg.data = range;    
-        range_pub.publish(&int_msg);  
-        pc.printf("%d\r\n", range);         
-        wait(0.5); 
-        nh.spinOnce();     
+        range_pub.publish(&int_msg);     
+        nh.spinOnce();      
+        wait(0.1); 
+        
     } 
 }