Soft robot team / Mbed 2 deprecated Nucleo_ros_motor_speed

Code to define toggle a led depending on a motor speed

Dependencies:   mbed ros_lib_kinetic

main.cpp@0:80d03f06b4ce, 2019-10-15 (annotated)

Committer:
florine_van
Date:
Tue Oct 15 09:05:09 2019 +0000
Revision:
0:80d03f06b4ce
intial revision;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
florine_van 0:80d03f06b4ce 1 #include "mbed.h"
florine_van 0:80d03f06b4ce 2 #include <ros.h>
florine_van 0:80d03f06b4ce 3 #include <std_msgs/Empty.h>
florine_van 0:80d03f06b4ce 4 #include <std_msgs/Int32.h>
florine_van 0:80d03f06b4ce 5
florine_van 0:80d03f06b4ce 6 ros::NodeHandle nh;
florine_van 0:80d03f06b4ce 7 DigitalOut myled(LED1);
florine_van 0:80d03f06b4ce 8 int speed;
florine_van 0:80d03f06b4ce 9
florine_van 0:80d03f06b4ce 10 void toggleSpeed(const std_msgs::Int32& speed_)
florine_van 0:80d03f06b4ce 11 {
florine_van 0:80d03f06b4ce 12 speed = speed_.data;
florine_van 0:80d03f06b4ce 13 }
florine_van 0:80d03f06b4ce 14
florine_van 0:80d03f06b4ce 15 ros::Subscriber<std_msgs::Int32> sub("motor_speed", &toggleSpeed);
florine_van 0:80d03f06b4ce 16
florine_van 0:80d03f06b4ce 17 int main()
florine_van 0:80d03f06b4ce 18 {
florine_van 0:80d03f06b4ce 19 nh.initNode();
florine_van 0:80d03f06b4ce 20 nh.subscribe(sub);
florine_van 0:80d03f06b4ce 21
florine_van 0:80d03f06b4ce 22 while (1)
florine_van 0:80d03f06b4ce 23 {
florine_van 0:80d03f06b4ce 24 myled = !myled;
florine_van 0:80d03f06b4ce 25 nh.spinOnce();
florine_van 0:80d03f06b4ce 26
florine_van 0:80d03f06b4ce 27 if (speed <= 20){wait(2.0);}
florine_van 0:80d03f06b4ce 28 else if (speed <= 40){wait(1);}
florine_van 0:80d03f06b4ce 29 else if (speed <= 60){wait_ms(500);}
florine_van 0:80d03f06b4ce 30 else {wait_ms(100);}
florine_van 0:80d03f06b4ce 31 }
florine_van 0:80d03f06b4ce 32 }