From f91b510e9d1bc314f34d022ba77519ef3c7ac71c Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Fri, 8 Nov 2019 14:56:58 -0800
Subject: [PATCH] Get servos to work

---
 src/uaspire_controls/CMakeLists.txt | 11 ++--------
 src/uaspire_controls/main.cc        | 32 +++++++++++++++++++++--------
 2 files changed, 26 insertions(+), 17 deletions(-)

diff --git a/src/uaspire_controls/CMakeLists.txt b/src/uaspire_controls/CMakeLists.txt
index 236833f..db614be 100644
--- a/src/uaspire_controls/CMakeLists.txt
+++ b/src/uaspire_controls/CMakeLists.txt
@@ -29,15 +29,8 @@ find_package(std_msgs REQUIRED)
 find_package(uaspire_msgs REQUIRED)
 
 if (BUILD_RASPBERRY_PI)
-    find_library(WIRINGPI_LIB NAMES wiringPi HINTS ${CMAKE_SOURCE_DIR}/../WiringPi/wiringPi)
-    find_path(WIRINGPI_INCLUDE_DIR NAMES wiringPi.h HINTS ${CMAKE_SOURCE_DIR}/../WiringPi/wiringPi)
-    if (NOT WIRINGPI_LIB OR NOT WIRINGPI_INCLUDE_DIR)
-        message(FATAL_ERROR "Could not find wiringPi: ${WIRINGPI_LIB} ${WIRINGPI_INCLUDE_DIR}")
-    else()
-        message(STATUS "Found wiringPi: ${WIRINGPI_LIB} ${WIRINGPI_INCLUDE_DIR}")
-    endif()
-    find_library(PIGPIO_LIB NAMES pigpiod_if HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
-    find_path(PIGPIO_INCLUDE_DIR NAMES pigpiod_if.h HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
+    find_library(PIGPIO_LIB NAMES pigpiod_if2 HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
+    find_path(PIGPIO_INCLUDE_DIR NAMES pigpiod_if2.h HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
     if (NOT PIGPIO_LIB OR NOT PIGPIO_INCLUDE_DIR)
         message(FATAL_ERROR "Could not find pigpio: ${PIGPIO_LIB} ${PIGPIO_INCLUDE_DIR}")
     else()
diff --git a/src/uaspire_controls/main.cc b/src/uaspire_controls/main.cc
index fae2246..430efe8 100644
--- a/src/uaspire_controls/main.cc
+++ b/src/uaspire_controls/main.cc
@@ -51,11 +51,17 @@ public:
   }
 
   void init_servos() {
-    _servos = { Servo { 9 }, Servo { 25 } };
+    _servos = { Servo { 23 }, Servo { 24 } };
     for (auto& servo : _servos) {
 #ifdef BUILD_RASPBERRY_PI
-      set_mode(_pi, servo.pin, PI_OUTPUT);
-      gpio_write(_pi, servo.pin, PI_LOW);
+      int result = set_mode(_pi, servo.pin, PI_OUTPUT);
+      if (result != 0) {
+        RCLCPP_ERROR(get_logger(), "Error setting pin mode: %d", result);
+      }
+      result = gpio_write(_pi, servo.pin, PI_LOW);
+      if (result != 0) {
+        RCLCPP_ERROR(get_logger(), "Error writing pin: %d", result);
+      }
 #endif
     }
   }
@@ -70,9 +76,12 @@ public:
       value = std::max(value, -90.f);
       // convert value from -90 to 90 to int from 1000 to 2000 (us)
       unsigned int decoded_value = static_cast<unsigned int>(
-          ((value / 90.f) * 500.f) + 1000);
+          ((value / 90.f) * 500.f) + 1500);
 #ifdef BUILD_RASPBERRY_PI
-      set_servo_pulsewidth(_pi, servo.pin, decoded_value);
+      int result = set_servo_pulsewidth(_pi, servo.pin, decoded_value);
+      if (result != 0) {
+        RCLCPP_ERROR(get_logger(), "error setting servo pulse width: %d", result);
+      }
 #endif
     }
   }
@@ -103,14 +112,21 @@ public:
                                std::bind(&ControlsNode::on_loop, this));
 
     _last_value = 0;
-    // start 100Hz servo timer
-    _servo_timer = create_wall_timer(std::chrono::milliseconds(10),
-                               std::bind(&ControlsNode::on_servo_update, this));
   }
 
 
   void on_message(const UaspireCommand::SharedPtr cmd) {
     RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str());
+    if (cmd->data == "start") {
+      // start 25Hz servo timer
+      _servo_timer = create_wall_timer(std::chrono::milliseconds(40),
+          std::bind(&ControlsNode::on_servo_update, this));
+    } else if (cmd->data == "stop") {
+      if (_servo_timer) {
+        _servo_timer->cancel();
+      }
+      _servo_timer.reset();
+    }
   }
 
   void on_loop() {