From 692abf10b514ced6ac5339f36e8488a4ab467bdd Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Fri, 6 Dec 2024 12:24:54 -0500
Subject: [PATCH] servo control

---
 .../include/modelec/gamecontroller_listener.hpp   |  2 ++
 src/modelec/src/arm_controller.cpp                |  2 +-
 src/modelec/src/gamecontroller_listener.cpp       | 15 +++++++++++++++
 3 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/src/modelec/include/modelec/gamecontroller_listener.hpp b/src/modelec/include/modelec/gamecontroller_listener.hpp
index 6bc72f1..dc859c2 100644
--- a/src/modelec/include/modelec/gamecontroller_listener.hpp
+++ b/src/modelec/include/modelec/gamecontroller_listener.hpp
@@ -9,6 +9,7 @@
 #include <sensor_msgs/msg/joy.hpp>
 #include <modelec_interface/msg/servo_mode.hpp>
 #include <modelec_interface/msg/pca9685_servo.hpp>
+#include <modelec_interface/srv/new_servo_motor.hpp>
 #include <modelec/utils.hpp>
 
 namespace Modelec {
@@ -45,6 +46,7 @@ namespace Modelec {
         rclcpp::Publisher<std_msgs::msg::String>::SharedPtr arduino_publisher_;
         rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr clear_pca_publisher_;
         rclcpp::Publisher<modelec_interface::msg::PCA9685Servo>::SharedPtr pca9685_publisher_;
+        rclcpp::Client<modelec_interface::srv::NewServoMotor>::SharedPtr client_;
 
         void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg);
         void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg);
diff --git a/src/modelec/src/arm_controller.cpp b/src/modelec/src/arm_controller.cpp
index 966478d..366d655 100644
--- a/src/modelec/src/arm_controller.cpp
+++ b/src/modelec/src/arm_controller.cpp
@@ -6,7 +6,7 @@ namespace Modelec {
         publisher_ = this->create_publisher<modelec_interface::msg::PCA9685Servo>("servo_control", 10);
         client_ = this->create_client<modelec_interface::srv::NewServoMotor>("add_servo");
 
-        for (int pin : {PINCE_1_PIN, PINCE_2_PIN, PINCE_3_PIN}) {
+        for (int pin : {PINCE_1_PIN, PINCE_2_PIN, PINCE_3_PIN, ARM_1_PIN, ARM_2_PIN}) {
             auto request = std::make_shared<modelec_interface::srv::NewServoMotor::Request>();
             request->pin = pin;
             auto res = client_->async_send_request(request);
diff --git a/src/modelec/src/gamecontroller_listener.cpp b/src/modelec/src/gamecontroller_listener.cpp
index a9c836c..bf13c85 100644
--- a/src/modelec/src/gamecontroller_listener.cpp
+++ b/src/modelec/src/gamecontroller_listener.cpp
@@ -16,6 +16,21 @@ namespace Modelec {
         clear_pca_publisher_ = this->create_publisher<std_msgs::msg::Empty>("clear_pca9685", 10);
 
         pca9685_publisher_ = this->create_publisher<modelec_interface::msg::PCA9685Servo>("servo_control", 10);
+
+        client_ = this->create_client<modelec_interface::srv::NewServoMotor>("add_servo");
+
+        for (auto servo : solarPannelServos) {
+            auto request = std::make_shared<modelec_interface::srv::NewServoMotor::Request>();
+            request->pin = servo.pin;
+            auto res = client_->async_send_request(request);
+            if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) == rclcpp::FutureReturnCode::SUCCESS) {
+                if (!res.get()->success) {
+                    RCLCPP_ERROR(this->get_logger(), "Failed to add servo on pin %d", servo.pin);
+                }
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "Service call failed");
+            }
+        }
     }
 
     void ControllerListener::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg)