modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

Add solar panel servo control to game controller listener

Browse Source
This commit is contained in:
acki
2024-12-05 20:54:23 -05:00
parent 70eb8aa1be
commit d081154ebb
3 changed files with 36 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/modelec/include/modelec/arm_controller.hpp
View File

@@ -23,6 +23,7 @@ namespace Modelec {
struct Pince { struct Pince {
int pin; int pin;
int mode; int mode;
// angle mapping : 1 - PINCE_CLOSED | 2 - PINCE_MIDDLE | 3 - PINCE_OPEN | 4 - PINCE_FULLY_OPEN
std::array<int, 4> angles; std::array<int, 4> angles;
}; };

17
src/modelec/include/modelec/gamecontroller_listener.hpp
View File

@@ -1,11 +1,14 @@
#pragma once #pragma once
#include "modelec_interface/msg/pca9685_servo.hpp"
#include <array>
#include <rclcpp/publisher.hpp> #include <rclcpp/publisher.hpp>
#include <std_msgs/msg/string.hpp> #include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp> #include <std_msgs/msg/empty.hpp>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joy.hpp> #include <sensor_msgs/msg/joy.hpp>
#include <modelec_interface/msg/servo_mode.hpp> #include <modelec_interface/msg/servo_mode.hpp>
#include <modelec_interface/msg/pca9685_servo.hpp>
#include <modelec/utils.hpp> #include <modelec/utils.hpp>
namespace Modelec { namespace Modelec {
@@ -20,6 +23,17 @@ namespace Modelec {
}; };
int arm = ServoMode::ARM_BOTTOM; 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: public:
ControllerListener(); ControllerListener();
@@ -30,6 +44,7 @@ namespace Modelec {
rclcpp::Publisher<ServoMode>::SharedPtr servo_publisher_; rclcpp::Publisher<ServoMode>::SharedPtr servo_publisher_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr arduino_publisher_; rclcpp::Publisher<std_msgs::msg::String>::SharedPtr arduino_publisher_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr clear_pca_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 CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg);
void CheckAxis(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_speed = 0;
int last_rotation = 0; int last_rotation = 0;
int last_solar_1_angle = 0;
int last_solar_2_angle = 0;
}; };
} }

18
src/modelec/src/gamecontroller_listener.cpp
View File

@@ -148,6 +148,24 @@ namespace Modelec {
arduino_publisher_->publish(message); arduino_publisher_->publish(message);
last_rotation = rotation; 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;
}
} }
} }