Diff: main.cpp

Revision:

4:5170ec66aabb

Parent:

3:3a65e8aae6b6

Child:

5:4a24d8597fc3

--- a/main.cpp	Tue Oct 15 12:20:06 2019 +0000
+++ b/main.cpp	Wed Oct 16 15:44:14 2019 +0000
@@ -1,15 +1,16 @@
-/*
- * rosserial Publisher Example
- * Prints "hello world!"
- */
+
 
 #include"mbed.h"
 #include <ros.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Int32.h>
 
+Serial pc(SERIAL_TX, SERIAL_RX); // set-up serial to pc
 DigitalOut led = LED1;
-DigitalOut SHDN_1(D0);
+DigitalOut SHDN_1(PC_9);
+DigitalOut SHDN_2(PC_11);
+DigitalOut SHDN_3(PD_2);
+
 
 // set-up serial to pc 
 I2C i2c(I2C_SDA, I2C_SCL); 
@@ -53,10 +54,7 @@
 // load settings 
 /////////////////////////////////////////////////////////////////// 
 int VL6180_Init(char addr) {    
-    char reset;
-
-    //WriteByte(0x212, addr/2, 0x52);
-    
+    char reset;    
     
     reset = ReadByte(0x016, addr);    
     if (reset==1)
@@ -149,7 +147,7 @@
 /////////////////////////////////////////////////////////////////// 
 int VL6180_Clear_Interrupts() {    
     WriteByte(0x015,0x07, addr1);    
-    WriteByte(0x015,0x07, addr2);  
+  //  WriteByte(0x015,0x07, addr2);  
     return 0; 
 }
 
@@ -158,19 +156,21 @@
 /////////////////////////////////////////////////////////////////// 
 int main() {   
  
-    ros::NodeHandle  nh;
-    nh.initNode();
-    
-    std_msgs::Int32 int_sensor1_msg;
-    std_msgs::Int32 int_sensor2_msg;
-    ros::Publisher range1_pub("sensor1", &int_sensor1_msg);
-    ros::Publisher range2_pub("sensor2", &int_sensor2_msg);
+       SHDN_3 =0;
+       SHDN_2 =0;
+//    ros::NodeHandle  nh;
+//    nh.initNode();
+//    
+//    std_msgs::Int32 int_sensor1_msg;
+//    std_msgs::Int32 int_sensor2_msg;
+//    ros::Publisher range1_pub("sensor1", &int_sensor1_msg);
 
-    nh.advertise(range1_pub);
-    nh.advertise(range2_pub);
+//
+//    nh.advertise(range1_pub);
+//    nh.advertise(range2_pub);
     
     int range1; 
-    int range2;
+     int range2;
     
     SHDN_1 = 0;
     
@@ -202,11 +202,13 @@
         VL6180_Clear_Interrupts();                 
         // send range to pc by serial
          
-        int_sensor1_msg.data = range1;
-        int_sensor2_msg.data = range2;    
-        range1_pub.publish(&int_sensor1_msg);     
-        range2_pub.publish(&int_sensor2_msg);  
-        nh.spinOnce();      
+//        int_sensor1_msg.data = range1;
+//        int_sensor2_msg.data = range2;    
+//        range1_pub.publish(&int_sensor1_msg);     
+//        range2_pub.publish(&int_sensor2_msg);  
+//        nh.spinOnce();
+       pc.printf("Range one =%d and range two = %d\r\n ",range1, range2);  
+       //pc.printf("Range one = %d\r\n ",range1);      
         wait(0.1); 
         
     }