diff --git a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp index 83b1553..16c09ca 100644 --- a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp +++ b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp @@ -40,7 +40,9 @@ class TalonSRXWrapper : public BaseWrapper { static void setup(); private: - enum class SensorType { PWM, RELATIVE, ANALOG }; + enum class SensorType { PWM, RELATIVE, ANALOG, NONE }; + static SensorType sensor_type_from_str(std::string str); + int get_load_enc() const; // Parameters int id_; @@ -51,6 +53,7 @@ class TalonSRXWrapper : public BaseWrapper { double sensor_offset_ticks_; ctre::phoenix::motorcontrol::ControlMode control_type_; SensorType sensor_type_; + SensorType load_sensor_; int sensor_ticks_; bool crossover_mode_; bool inverted_; @@ -59,6 +62,11 @@ class TalonSRXWrapper : public BaseWrapper { rclcpp::Time start_time_; rclcpp::Publisher::SharedPtr debug_pub_; static constexpr double kWaitDurationSec = 5.0; + static inline std::map sensor_type_map = { + {"quadrature", SensorType::RELATIVE}, + {"absolute", SensorType::PWM}, + {"analog", SensorType::ANALOG}, + {"none", SensorType::NONE}}; // Talon-specific handle std::shared_ptr talon_controller_; diff --git a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp index 15cc5a9..e7cc731 100644 --- a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp +++ b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp @@ -18,7 +18,9 @@ TalonSRXWrapper::TalonSRXWrapper(const hardware_interface::ComponentInfo &joint, if (param.first == "can_id") { id_ = std::stoi(param.second); } else if (param.first == "sensor_type") { - sensor_type_str = param.second; + sensor_type_ = sensor_type_from_str(param.second); + } else if (param.first == "load_sensor") { + load_sensor_ = sensor_type_from_str(param.second); } else if (param.first == "sensor_ticks") { sensor_ticks_ = std::stoi(param.second); } else if (param.first == "sensor_offset") { @@ -77,20 +79,21 @@ TalonSRXWrapper::TalonSRXWrapper(const hardware_interface::ComponentInfo &joint, debug_pub_ = debug_node_->create_publisher( joint.name + "/status", rclcpp::SystemDefaultsQoS()); - std::transform(sensor_type_str.begin(), sensor_type_str.end(), - sensor_type_str.begin(), - [](unsigned char c) { return std::tolower(c); }); - if (sensor_type_str == "quadrature") { - sensor_type_ = SensorType::RELATIVE; - } else if (sensor_type_str == "absolute") { - sensor_type_ = SensorType::PWM; - } else if (sensor_type_str == "analog") { - sensor_type_ = SensorType::ANALOG; - } sensor_offset_ticks_ = static_cast(sensor_offset * sensor_ticks_ / (2.0 * M_PI)); } +TalonSRXWrapper::SensorType +TalonSRXWrapper::sensor_type_from_str(std::string str) { + std::transform(str.begin(), str.end(), str.begin(), + [](unsigned char c) { return std::tolower(c); }); + const auto itr = sensor_type_map.find(str); + if (itr == sensor_type_map.end()) { + return SensorType::NONE; + } + return itr->second; +} + void TalonSRXWrapper::pub_status() const { if (!talon_controller_ || !debug_pub_) { return; @@ -176,6 +179,21 @@ void TalonSRXWrapper::read() { velocity_ = raw_velocity * 10 * (2.0 * M_PI) / sensor_ticks_; } +int TalonSRXWrapper::get_load_enc() const { + auto &sensor_collection = talon_controller_->GetSensorCollection(); + int abs_ticks = 0; + switch (load_sensor_) { + case SensorType::PWM: + return sensor_collection.GetPulseWidthPosition(); + case SensorType::ANALOG: + return sensor_collection.GetAnalogIn(); + case SensorType::RELATIVE: + return sensor_collection.GetQuadraturePosition(); + default: + return 0; + } +} + void TalonSRXWrapper::configure() { BaseWrapper::configure(); while (true) { @@ -233,6 +251,9 @@ void TalonSRXWrapper::configure() { talon_controller_->SetStatusFramePeriod( StatusFrameEnhanced::Status_2_Feedback0, 20); talon_controller_->SetIntegralAccumulator(0); + if (load_sensor_ != SensorType::NONE) { + talon_controller_->SetSelectedSensorPosition(get_load_enc()); + } read(); if (control_type_ == motors::ControlMode::Position) { command_ = position_; diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro index 92dd716..4d4e4b3 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro @@ -14,9 +14,10 @@ 0.0005 1.0 0.0 - absolute + quadrature + absolute 4096 - 0.0 + 4.99 false false false @@ -31,9 +32,10 @@ 0.001 0.01 0.0 - absolute + quadrature + absolute 4096 - 0.0 + 1.9 false false false @@ -48,9 +50,10 @@ 0.001 0.0 0.0 - absolute + quadrature + absolute 4096 - 0.0 + 4.69494 false true true @@ -65,9 +68,10 @@ 0.001 0.001 0.0 - absolute + quadrature + absolute 4096 - 0.0 + 3.26377 false false false