Diff: main.cpp

Revision:

0:7107a8b171d1

Child:

1:e3660c13da3c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 03 11:43:08 2019 +0000
@@ -0,0 +1,62 @@
+#include "mbed.h"
+//#include "VL6180.h"
+//#include <std_msgs/Empty.h>
+#include <ros.h>
+#include <std_msgs/UInt8.h>
+///////////////////////////////////////////////////////////////////
+// Beginning of code
+///////////////////////////////////////////////////////////////////
+//#include "timeofflight.hpp"
+#define addr (0x52)
+#define fwd 1
+
+Serial pc(USBTX,USBRX);
+//VL6180 distance_sensor (SDA,SCL);
+//Serial pc(SERIAL_TX, SERIAL_RX);// set-up serial to pc
+            // I²C address of VL6180 shifted by 1 bit
+/*
+VL6180 tof1(0x52);
+VL6180 tof2(0x53);
+VL6180 tof3(0x54);
+VL6180 tof4(0x55);
+*/
+
+ros::NodeHandle  nh;
+
+std_msgs::UInt8 Tof1;
+ros::Publisher tof1("tof1", &Tof1);
+
+
+std_msgs::UInt8 Tof2;
+ros::Publisher tof2("tof2", &Tof2);
+
+std_msgs::UInt8 Tof3;
+ros::Publisher tof3("tof3", &Tof3);
+
+std_msgs::UInt8 Tof4;
+ros::Publisher tof4("tof4", &Tof4);
+
+
+int main() {
+    nh.initNode();
+    nh.advertise(tof1);
+    nh.advertise(tof2);
+    nh.advertise(tof3);
+    nh.advertise(tof4);
+   
+    while (1) {
+//        led = !led;
+        Tof1.data = 5;
+        tof1.publish( &Tof1 );
+        Tof2.data = 6;
+        tof2.publish( &Tof2 );
+        Tof2.data = 7;
+        tof2.publish( &Tof3);
+        Tof2.data = 8;
+        tof2.publish( &Tof4);
+        nh.spinOnce();
+        wait_ms(1000);
+    }
+}
+
+