button_example.cpp
Go to the documentation of this file.
00001 /*
00002  * Button Example for Rosserial
00003  */
00004 
00005 #include "mbed.h"
00006 #include <ros.h>
00007 #include <std_msgs/Bool.h>
00008 
00009 #ifdef TARGET_LPC1768
00010 PinName button = p20;
00011 #elif defined(TARGET_KL25Z)
00012 PinName button = D10;
00013 #elif defined(TARGET_NUCLEO_F401RE)
00014 PinName button = USER_BUTTON;
00015 #else
00016 #error "You need to specify a pin for the button"
00017 #endif
00018 
00019 ros::NodeHandle nh;
00020 
00021 std_msgs::Bool pushed_msg;
00022 ros::Publisher pub_button("pushed", &pushed_msg);
00023 
00024 DigitalIn button_pin(button);
00025 DigitalOut led_pin(LED1);
00026 
00027 bool last_reading;
00028 long last_debounce_time=0;
00029 long debounce_delay=50;
00030 bool published = true;
00031 
00032 Timer t;
00033 int main() {
00034     t.start();
00035     nh.initNode();
00036     nh.advertise(pub_button);
00037 
00038     //Enable the pullup resistor on the button
00039     button_pin.mode(PullUp);
00040 
00041     //The button is a normally button
00042     last_reading = ! button_pin;
00043 
00044     while (1) {
00045         bool reading = ! button_pin;
00046 
00047         if (last_reading!= reading) {
00048             last_debounce_time = t.read_ms();
00049             published = false;
00050         }
00051 
00052         //if the button value has not changed for the debounce delay, we know its stable
00053         if ( !published && (t.read_ms() - last_debounce_time)  > debounce_delay) {
00054             led_pin = reading;
00055             pushed_msg.data = reading;
00056             pub_button.publish(&pushed_msg);
00057             published = true;
00058         }
00059 
00060         last_reading = reading;
00061 
00062         nh.spinOnce();
00063     }
00064 }
00065 


rosserial_mbed
Author(s): Gary Servin
autogenerated on Sat Oct 7 2017 03:08:46