Added estop

ihe_hardware/turtlebot_button/README

@@ -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

ihe_hardware/turtlebot_button/buttons_turtlebot/buttons_turtlebot.ino

@@ -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();
+}

ihe_hardware/turtlebot_button/msg/Buttons.msg

@@ -1,2 +1,3 @@
 
-bool[] button_state
+uint8[] button_state
+uint8 estop_state