From 0ab4fa6243189408de344ef858e711b9b3c120cb Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@husarion.com>
Date: Fri, 23 Jun 2023 18:27:33 +0200
Subject: [PATCH] changed buttons publish to rise and fall

---
 include/main.hpp |  1 -
 src/main.cpp     | 29 +++++++++++++++++------------
 src/microros.cpp |  3 ++-
 3 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/include/main.hpp b/include/main.hpp
index 237713c..313cb8a 100644
--- a/include/main.hpp
+++ b/include/main.hpp
@@ -54,7 +54,6 @@ std::map<double, uint8_t> servo_voltage_configuration{
 #define MOTOR_FL MOTOR4
 #define MOTOR_RR MOTOR2
 #define MOTOR_RL MOTOR3
-time_t last_button_press_time[BUTTONS_COUNT];
 
 constexpr uint8_t POLARITY = 0b00111100;
 constexpr float ROBOT_LENGTH = 0.197;
diff --git a/src/main.cpp b/src/main.cpp
index 655ace2..1236f53 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -35,14 +35,22 @@ std_msgs__msg__Bool button_msgs[BUTTONS_COUNT];
 
 static uint32_t spin_count = 1;
 
-static void button1Callback() {
+static void button1FallCallback() {
     buttons_publish_flag[0] = true;
 }
 
-static void button2Callback() {
+static void button2FallCallback() {
     buttons_publish_flag[1] = true;
 }
 
+static void button1RiseCallback() {
+    buttons_publish_flag[0] = false;
+}
+
+static void button2RiseCallback() {
+    buttons_publish_flag[1] = false;
+}
+
 void range_sensors_msg_handler() {
     osEvent evt1 = distance_sensor_mail_box.get(0);
     if (evt1.status == osEventMail) {
@@ -112,14 +120,8 @@ void battery_msg_handler() {
 
 void button_msgs_handler() {
     for(auto i = 0u; i < BUTTONS_COUNT; ++i){
-        if (buttons_publish_flag[i]) {
-            button_msgs[i].data = true;
-            buttons_publish_flag[i] = false;
-            publish_button_msg(&button_msgs[i], i);
-            last_button_press_time[i] = time(NULL);
-        }
-        else if(time(NULL) - last_button_press_time[i]  > button_release_time){
-            button_msgs[i].data = false;
+        if (buttons_publish_flag[i] != button_msgs[i].data) {
+            button_msgs[i].data = buttons_publish_flag[i];
             publish_button_msg(&button_msgs[i], i);
         }
     }
@@ -309,8 +311,11 @@ int main() {
 
     button1.mode(PullUp);
     button2.mode(PullUp);
-    button1.fall(button1Callback);
-    button2.fall(button2Callback);
+    button1.fall(button1FallCallback);
+    button2.fall(button2FallCallback);
+    button1.rise(button1RiseCallback);
+    button2.rise(button2RiseCallback);
+
 
     bool distance_sensors_init_flag = false;
     bool imu_init_flag = false;
diff --git a/src/microros.cpp b/src/microros.cpp
index 44b50e3..293ac69 100644
--- a/src/microros.cpp
+++ b/src/microros.cpp
@@ -45,6 +45,7 @@ bool microros_init() {
     rcl_allocator = rcl_get_default_allocator();
     RCCHECK(rclc_support_init(&support, 0, NULL, &rcl_allocator));
     RCCHECK(rclc_node_init_default(&node, NODE_NAME, "", &support));
+
     if (not init_wheels_command_subscriber() or
         not init_servos_command_subscriber() or
         not init_wheels_state_publisher() or
@@ -151,7 +152,7 @@ bool init_range_publishers() {
 
 bool init_button_publishers() {
     for (auto i = 0u; i < BUTTONS_COUNT; ++i) {
-        RCCHECK(rclc_publisher_init_best_effort(
+        RCCHECK(rclc_publisher_init_default(
             &buttons_pubs[i],
             &node,
             ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),