Code to define toggle a led depending on a motor speed

Dependencies:   mbed ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <std_msgs/Empty.h>
00004 #include <std_msgs/Int32.h>
00005 
00006 ros::NodeHandle nh;
00007 DigitalOut myled(LED1);
00008 int speed;
00009 
00010 void toggleSpeed(const std_msgs::Int32& speed_)
00011 {
00012     speed = speed_.data;
00013 }
00014  
00015 ros::Subscriber<std_msgs::Int32> sub("motor_speed", &toggleSpeed);
00016 
00017 int main() 
00018 {
00019     nh.initNode();
00020     nh.subscribe(sub);
00021      
00022     while (1) 
00023     {
00024         myled = !myled;
00025         nh.spinOnce();
00026         
00027         if (speed <= 20){wait(2.0);}
00028         else if (speed <= 40){wait(1);}
00029         else if (speed <= 60){wait_ms(500);}
00030         else {wait_ms(100);}
00031     }
00032 }