Skip to content

Commit

Permalink
upd
Browse files Browse the repository at this point in the history
  • Loading branch information
Naufaldo committed Sep 18, 2021
1 parent ef7f628 commit 9da9fd5
Showing 1 changed file with 1 addition and 5 deletions.
6 changes: 1 addition & 5 deletions Arduino/Button_gripper/Button_gripper.ino
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
#include <std_msgs/UInt16.h>
#include <std_msgs/Bool.h>

ros::NodeHandle_<ArduinoHardware, 2, 2, 80, 105> nh;
ros::NodeHandle_<ArduinoHardware, 2, 3, 80, 105> nh;

Servo servo;

Expand All @@ -39,18 +39,15 @@ std_msgs::Bool pushed_msg;
ros::Publisher pub_button("pushed", &pushed_msg);

const int button_pin = 4;
const int led_pin = 8;

bool last_reading;
long last_debounce_time=0;
long debounce_delay=50;
bool published = true;

void setup(){
pinMode(13, OUTPUT);
//initialize an LED output pin
//and a input pin for our push button
pinMode(led_pin, OUTPUT);
pinMode(button_pin, INPUT);
pinMode(6, OUTPUT); // LED POWER
//Enable the pullup resistor on the button
Expand All @@ -76,7 +73,6 @@ void loop(){

//if the button value has not changed for the debounce delay, we know its stable
if ( !published && (millis() - last_debounce_time) > debounce_delay) {
digitalWrite(led_pin, reading);
pushed_msg.data = reading;
pub_button.publish(&pushed_msg);
published = true;
Expand Down

0 comments on commit 9da9fd5

Please sign in to comment.