From d081154ebb9441c2c0ce15ee2b36abfbc989a697 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Thu, 5 Dec 2024 20:54:23 -0500
Subject: [PATCH] Add solar panel servo control to game controller listener

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

diff --git a/src/modelec/include/modelec/arm_controller.hpp b/src/modelec/include/modelec/arm_controller.hpp
index cd86746..63dd6ea 100644
--- a/src/modelec/include/modelec/arm_controller.hpp
+++ b/src/modelec/include/modelec/arm_controller.hpp
@@ -23,6 +23,7 @@ namespace Modelec {
         struct Pince {
             int pin;
             int mode;
+            // angle mapping : 1 - PINCE_CLOSED | 2 - PINCE_MIDDLE | 3 - PINCE_OPEN | 4 - PINCE_FULLY_OPEN
             std::array<int, 4> angles;
         };
 
diff --git a/src/modelec/include/modelec/gamecontroller_listener.hpp b/src/modelec/include/modelec/gamecontroller_listener.hpp
index 775cf18..5f96d4b 100644
--- a/src/modelec/include/modelec/gamecontroller_listener.hpp
+++ b/src/modelec/include/modelec/gamecontroller_listener.hpp
@@ -1,11 +1,14 @@
 #pragma once
 
+#include "modelec_interface/msg/pca9685_servo.hpp"
+#include <array>
 #include <rclcpp/publisher.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <std_msgs/msg/empty.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joy.hpp>
 #include <modelec_interface/msg/servo_mode.hpp>
+#include <modelec_interface/msg/pca9685_servo.hpp>
 #include <modelec/utils.hpp>
 
 namespace Modelec {
@@ -20,6 +23,17 @@ namespace Modelec {
         };
 
         int arm = ServoMode::ARM_BOTTOM;
+
+        struct SolarPannelServo {
+            int pin;
+            float startAngle;
+            float endAngle;
+        };
+        
+        std::array<SolarPannelServo, 2> solarPannelServos = {
+            { { 6, 16, 40 }, { 7, 25, 3 } }
+        };
+
     public:
         ControllerListener();
 
@@ -30,6 +44,7 @@ namespace Modelec {
         rclcpp::Publisher<ServoMode>::SharedPtr servo_publisher_;
         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_;
 
         void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg);
         void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg);
@@ -43,5 +58,7 @@ namespace Modelec {
 
         int last_speed = 0;
         int last_rotation = 0;
+        int last_solar_1_angle = 0;
+        int last_solar_2_angle = 0;
     };
 }
diff --git a/src/modelec/src/gamecontroller_listener.cpp b/src/modelec/src/gamecontroller_listener.cpp
index fac51b6..a9fb0ff 100644
--- a/src/modelec/src/gamecontroller_listener.cpp
+++ b/src/modelec/src/gamecontroller_listener.cpp
@@ -148,6 +148,24 @@ namespace Modelec {
             arduino_publisher_->publish(message);
             last_rotation = rotation;
         }
+
+        if (msg->axes[4] != last_solar_1_angle) {
+            int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[4]), -1.0f, 1.0f, solarPannelServos[0].startAngle, solarPannelServos[0].endAngle));
+            auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo();
+            solarPannelAngleMessage.pin = solarPannelServos[0].pin;
+            solarPannelAngleMessage.angle = solarPannelAngle;
+            pca9685_publisher_->publish(solarPannelAngleMessage);
+            last_solar_1_angle = solarPannelAngle;
+        }
+
+        if (msg->axes[5] != last_solar_2_angle) {
+            int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[5]), -1.0f, 1.0f, solarPannelServos[1].startAngle, solarPannelServos[1].endAngle));
+            auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo();
+            solarPannelAngleMessage.pin = solarPannelServos[1].pin;
+            solarPannelAngleMessage.angle = solarPannelAngle;
+            pca9685_publisher_->publish(solarPannelAngleMessage);
+            last_solar_2_angle = solarPannelAngle;
+        }
     }
 }