SOFT564 / Mbed 2 deprecated Nucleo_blink_ledse

5r

Dependencies:   mbed ros_lib_kinetic

main.cpp@1:e3660c13da3c, 2019-12-11 (annotated)

Committer:
msalmehmadi
Date:
Wed Dec 11 14:54:04 2019 +0000
Revision:
1:e3660c13da3c
Parent:
0:7107a8b171d1 
ready to share

Who changed what in which revision?

UserRevisionLine numberNew contents of line
msalmehmadi 0:7107a8b171d1 1 #include "mbed.h"
msalmehmadi 0:7107a8b171d1 2 //#include "VL6180.h"
msalmehmadi 1:e3660c13da3c 3 #include <std_msgs/Empty.h>
msalmehmadi 0:7107a8b171d1 4 #include <ros.h>
msalmehmadi 0:7107a8b171d1 5 #include <std_msgs/UInt8.h>
msalmehmadi 0:7107a8b171d1 6 ///////////////////////////////////////////////////////////////////
msalmehmadi 0:7107a8b171d1 7 // Beginning of code
msalmehmadi 0:7107a8b171d1 8 ///////////////////////////////////////////////////////////////////
msalmehmadi 0:7107a8b171d1 9 //#include "timeofflight.hpp"
msalmehmadi 0:7107a8b171d1 10 #define addr (0x52)
msalmehmadi 0:7107a8b171d1 11 #define fwd 1
msalmehmadi 0:7107a8b171d1 12
msalmehmadi 0:7107a8b171d1 13 Serial pc(USBTX,USBRX);
msalmehmadi 0:7107a8b171d1 14 //VL6180 distance_sensor (SDA,SCL);
msalmehmadi 0:7107a8b171d1 15 //Serial pc(SERIAL_TX, SERIAL_RX);// set-up serial to pc
msalmehmadi 0:7107a8b171d1 16 // I²C address of VL6180 shifted by 1 bit
msalmehmadi 0:7107a8b171d1 17 /*
msalmehmadi 0:7107a8b171d1 18 VL6180 tof1(0x52);
msalmehmadi 0:7107a8b171d1 19 VL6180 tof2(0x53);
msalmehmadi 0:7107a8b171d1 20 VL6180 tof3(0x54);
msalmehmadi 0:7107a8b171d1 21 VL6180 tof4(0x55);
msalmehmadi 0:7107a8b171d1 22 */
msalmehmadi 0:7107a8b171d1 23
msalmehmadi 1:e3660c13da3c 24 DigitalOut myled(LED1);
msalmehmadi 1:e3660c13da3c 25
msalmehmadi 0:7107a8b171d1 26 ros::NodeHandle nh;
msalmehmadi 0:7107a8b171d1 27
msalmehmadi 0:7107a8b171d1 28 std_msgs::UInt8 Tof1;
msalmehmadi 1:e3660c13da3c 29 ros::Publisher ("tof1", &Tof1);
msalmehmadi 0:7107a8b171d1 30
msalmehmadi 0:7107a8b171d1 31
msalmehmadi 0:7107a8b171d1 32 std_msgs::UInt8 Tof2;
msalmehmadi 0:7107a8b171d1 33 ros::Publisher tof2("tof2", &Tof2);
msalmehmadi 0:7107a8b171d1 34
msalmehmadi 0:7107a8b171d1 35 std_msgs::UInt8 Tof3;
msalmehmadi 0:7107a8b171d1 36 ros::Publisher tof3("tof3", &Tof3);
msalmehmadi 0:7107a8b171d1 37
msalmehmadi 0:7107a8b171d1 38 std_msgs::UInt8 Tof4;
msalmehmadi 0:7107a8b171d1 39 ros::Publisher tof4("tof4", &Tof4);
msalmehmadi 0:7107a8b171d1 40
msalmehmadi 1:e3660c13da3c 41 std_msgs::UInt8 received_speed;
msalmehmadi 1:e3660c13da3c 42 ros::Publisher speed_publisher("speed_publisher", &received_speed);
msalmehmadi 1:e3660c13da3c 43
msalmehmadi 1:e3660c13da3c 44 std_msgs::UInt8 the_motor_speed;
msalmehmadi 1:e3660c13da3c 45
msalmehmadi 1:e3660c13da3c 46 void messageCb(const std_msgs::UInt8& speed){
msalmehmadi 1:e3660c13da3c 47 the_motor_speed = speed;
msalmehmadi 1:e3660c13da3c 48 }
msalmehmadi 1:e3660c13da3c 49 ros::Subscriber<std_msgs::UInt8> sub("motor_speed", &messageCb);
msalmehmadi 1:e3660c13da3c 50
msalmehmadi 1:e3660c13da3c 51 //void messageCb(const std_msgs::Empty& toggle_msg){
msalmehmadi 1:e3660c13da3c 52 // myled = !myled; // blink the led
msalmehmadi 1:e3660c13da3c 53 //}
msalmehmadi 1:e3660c13da3c 54 //ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb);
msalmehmadi 0:7107a8b171d1 55
msalmehmadi 0:7107a8b171d1 56 int main() {
msalmehmadi 0:7107a8b171d1 57 nh.initNode();
msalmehmadi 0:7107a8b171d1 58 nh.advertise(tof1);
msalmehmadi 0:7107a8b171d1 59 nh.advertise(tof2);
msalmehmadi 0:7107a8b171d1 60 nh.advertise(tof3);
msalmehmadi 0:7107a8b171d1 61 nh.advertise(tof4);
msalmehmadi 1:e3660c13da3c 62 nh.advertise(speed_publisher);
msalmehmadi 1:e3660c13da3c 63
msalmehmadi 1:e3660c13da3c 64 nh.subscribe(sub);
msalmehmadi 0:7107a8b171d1 65
msalmehmadi 0:7107a8b171d1 66 while (1) {
msalmehmadi 0:7107a8b171d1 67 // led = !led;
msalmehmadi 0:7107a8b171d1 68 Tof1.data = 5;
msalmehmadi 0:7107a8b171d1 69 tof1.publish( &Tof1 );
msalmehmadi 0:7107a8b171d1 70 Tof2.data = 6;
msalmehmadi 0:7107a8b171d1 71 tof2.publish( &Tof2 );
msalmehmadi 1:e3660c13da3c 72 Tof3.data = 7;
msalmehmadi 1:e3660c13da3c 73 tof3.publish( &Tof3);
msalmehmadi 1:e3660c13da3c 74 Tof4.data = 8;
msalmehmadi 1:e3660c13da3c 75 tof4.publish( &Tof4);
msalmehmadi 1:e3660c13da3c 76 speed_publisher.publish(&the_motor_speed);
msalmehmadi 0:7107a8b171d1 77 nh.spinOnce();
msalmehmadi 0:7107a8b171d1 78 wait_ms(1000);
msalmehmadi 0:7107a8b171d1 79 }
msalmehmadi 0:7107a8b171d1 80 }
msalmehmadi 0:7107a8b171d1 81
msalmehmadi 0:7107a8b171d1 82