Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Added estop

  • Loading branch information...
commit 8da727cd057ce290b6cc54fb91cecd95379024ea 1 parent fe1ce0a
Linda oscillare authored
15 ihe_hardware/turtlebot_button/README
View
@@ -1,10 +1,15 @@
-There is a 30 second delay before the correct messages are sent.
+Setup:
Add the header file for Button.msg to Arduino libraries:
-rosrun rosserial_client make_library.py PATH_TO_ARDUINO_LIBRARY turtlebot_button
+
+ rosrun rosserial_client make_library.py PATH_TO_ARDUINO_LIBRARY turtlebot_button
+
+
To run:
-1) Upload buttons_turtlebot.ino onto an Arduino.
-2) Run the rosserial client that forwards the message to the rest of ROS:
- rosrun rosserial_python serial_node.py USB_PORT
+1) Upload buttons_turtlebot/buttons_turtlebot.ino onto an Arduino. (Currently
+ not compatiable with Leonardo.)
+2) Run the rosserial client that forwards the message to the rest of ROS.
+
+ rosrun rosserial_python serial_node.py SERIAL_PORT
88 ihe_hardware/turtlebot_button/buttons_turtlebot.ino
View
@@ -1,88 +0,0 @@
-#include <ros.h>
-#include <vector>
-#include <turtlebot_button_op/Buttons.h>
-
-ros::NodeHandle nh;
-
-turtlebot_button_op::Buttons button_msg;
-ros::Publisher pub_button("buttons", &button_msg);
-
-const int num_buttons = 5;
-
-int button0 = 8;
-int button1 = 9;
-int button2 = 10;
-int button3 = 11;
-int button4 = 12;
-
-bool last_button_state[num_buttons];
-int last_debounce_time[num_buttons];
-int debounce_delay = 50;
-bool published = true;
-bool updated = false;
-
-int i;
-
-void setup() {
- nh.initNode();
-
- pinMode(button0, INPUT);
- pinMode(button1, INPUT);
- pinMode(button2, INPUT);
- pinMode(button3, INPUT);
- pinMode(button4, INPUT);
-
- // Allocate space for button states
- button_msg.button_state_length = num_buttons;
- button_msg.button_state = (bool *) malloc(sizeof(bool) * num_buttons);
-
- // Get initial button state
- for (i = 0; i < num_buttons; i++) {
- // Buttons are normally low.
- last_button_state[i] = digitalRead(button0+i);
- last_debounce_time[i] = 0;
-
- button_msg.button_state[i] = last_button_state[i];
- }
-
- nh.advertise(pub_button);
-}
-
-void loop() {
- int reading[num_buttons];
-
- for (i = 0; i < num_buttons; i++) {
- reading[i] = digitalRead(button0+i);
- }
-
-
- for (i = 0; i < num_buttons; i++) {
- if (reading[i] != last_button_state[i]) {
- last_debounce_time[i] = millis();
- published = false;
- updated = true;
- }
- }
-
- for (i = 0; i < num_buttons; i++) {
- // Store stable button values
- if (!published && (millis() - last_debounce_time[i]) > debounce_delay) {
- button_msg.button_state[i] = reading[i];
- }
- }
-
- // Publish updated button message
- if (updated && !published) {
- pub_button.publish(&button_msg);
- published = true;
- }
-
- // Update last button state
- for (i = 0; i < num_buttons; i++) {
- last_button_state[i] = reading[i];
- }
-
- updated = false;
-
- nh.spinOnce();
-}
96 ihe_hardware/turtlebot_button/buttons_turtlebot/buttons_turtlebot.ino
View
@@ -0,0 +1,96 @@
+#include <ros.h>
+#include <turtlebot_button/Buttons.h>
+
+ros::NodeHandle nh;
+
+turtlebot_button::Buttons button_msg;
+ros::Publisher pub_button("buttons", &button_msg);
+
+const int num_buttons = 5;
+
+const int estop = 7;
+const int button0 = 8;
+const int button1 = 9;
+const int button2 = 10;
+const int button3 = 11;
+const int button4 = 12;
+
+int last_debounce_time[num_buttons];
+int debounce_delay = 50;
+bool button_updated[num_buttons];
+bool published = true;
+
+int i;
+
+void setup() {
+ nh.initNode();
+ nh.advertise(pub_button);
+
+ pinMode(estop, INPUT);
+ pinMode(button0, INPUT);
+ pinMode(button1, INPUT);
+ pinMode(button2, INPUT);
+ pinMode(button3, INPUT);
+ pinMode(button4, INPUT);
+
+ // Allocate space for button states
+ button_msg.button_state_length = num_buttons;
+ button_msg.button_state = (uint8_t *) malloc(sizeof(uint8_t) * num_buttons);
+
+ // Get initial state
+ // Depressed estop returns 0; otherwise, 1
+ button_msg.estop_state = digitalRead(estop);
+
+ for (i = 0; i < num_buttons; i++) {
+ // Buttons are normally high.
+ button_msg.button_state[i] = !digitalRead(button0+i);
+ last_debounce_time[i] = 0;
+ button_updated[i] = false;
+ }
+
+ // 35-second delay before reliable data is published
+ delay(35000);
+}
+
+void loop() {
+ int button_reading[num_buttons];
+ int estop_reading;
+
+ // Get current state
+ estop_reading = digitalRead(estop);
+
+ for (i = 0; i < num_buttons; i++) {
+ button_reading[i] = !digitalRead(button0+i);
+ }
+
+ // Reset debounce timer if button state changed
+ for (i = 0; i < num_buttons; i++) {
+ if (button_reading[i] != button_msg.button_state[i]) {
+ last_debounce_time[i] = millis();
+ button_updated[i] = true;
+ }
+ }
+
+ // Store stable button values
+ // Publish new message only when values have been updated
+ for (i = 0; i < num_buttons; i++) {
+ if (button_updated[i] && (millis() - last_debounce_time[i]) > debounce_delay) {
+ button_msg.button_state[i] = button_reading[i];
+ button_updated[i] = false;
+ published = false;
+ }
+ }
+
+ if (estop_reading != button_msg.estop_state) {
+ button_msg.estop_state = estop_reading;
+ published = false;
+ }
+
+ // Publish updated button message
+ if (!published) {
+ pub_button.publish(&button_msg);
+ published = true;
+ }
+
+ nh.spinOnce();
+}
3  ihe_hardware/turtlebot_button/msg/Buttons.msg
View
@@ -1,2 +1,3 @@
-bool[] button_state
+uint8[] button_state
+uint8 estop_state
Please sign in to comment.
Something went wrong with that request. Please try again.