From d349a26d01fdad18db0f99a06ce6d1266d88a9c3 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Wed, 6 Nov 2024 15:12:15 -0500 Subject: [PATCH 1/7] Code styling updates for default tests --- .../include/puma_motor_driver/can_proto.hpp | 93 ++-- .../include/puma_motor_driver/driver.hpp | 42 +- .../puma_motor_driver/multi_puma_node.hpp | 46 +- puma_motor_driver/src/driver.cpp | 513 ++++++++---------- puma_motor_driver/src/multi_puma_node.cpp | 128 ++--- 5 files changed, 371 insertions(+), 451 deletions(-) diff --git a/puma_motor_driver/include/puma_motor_driver/can_proto.hpp b/puma_motor_driver/include/puma_motor_driver/can_proto.hpp index 0e4ac87..9a14fe2 100644 --- a/puma_motor_driver/include/puma_motor_driver/can_proto.hpp +++ b/puma_motor_driver/include/puma_motor_driver/can_proto.hpp @@ -130,8 +130,7 @@ // The Stellaris Motor Class Control Voltage API definitions. // //***************************************************************************** -#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_VOLTAGE) +#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE) #define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S)) #define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S)) #define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S)) @@ -157,8 +156,7 @@ // The Stellaris Motor Class Speed Control API definitions. // //***************************************************************************** -#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_SPD) +#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD) #define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S)) #define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S)) #define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S)) @@ -173,8 +171,7 @@ // The Stellaris Motor Control Voltage Compensation Control API definitions. // //***************************************************************************** -#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_VCOMP) +#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VCOMP) #define LM_API_VCOMP_EN (LM_API_VCOMP | (0 << CAN_MSGID_API_S)) #define LM_API_VCOMP_DIS (LM_API_VCOMP | (1 << CAN_MSGID_API_S)) #define LM_API_VCOMP_SET (LM_API_VCOMP | (2 << CAN_MSGID_API_S)) @@ -187,8 +184,7 @@ // The Stellaris Motor Class Position Control API definitions. // //***************************************************************************** -#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_POS) +#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS) #define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S)) #define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S)) #define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S)) @@ -203,8 +199,7 @@ // The Stellaris Motor Class Current Control API definitions. // //***************************************************************************** -#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_ICTRL) +#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ICTRL) #define LM_API_ICTRL_EN (LM_API_ICTRL | (0 << CAN_MSGID_API_S)) #define LM_API_ICTRL_DIS (LM_API_ICTRL | (1 << CAN_MSGID_API_S)) #define LM_API_ICTRL_SET (LM_API_ICTRL | (2 << CAN_MSGID_API_S)) @@ -232,8 +227,7 @@ // The Stellaris Motor Class Status API definitions. // //***************************************************************************** -#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_STATUS) +#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_STATUS) #define LM_API_STATUS_VOLTOUT (LM_API_STATUS | (0 << CAN_MSGID_API_S)) #define LM_API_STATUS_VOLTBUS (LM_API_STATUS | (1 << CAN_MSGID_API_S)) #define LM_API_STATUS_CURRENT (LM_API_STATUS | (2 << CAN_MSGID_API_S)) @@ -274,8 +268,7 @@ // The Stellaris Motor Class Configuration API definitions. // //***************************************************************************** -#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_CFG) +#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG) #define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S)) #define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S)) #define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S)) @@ -298,8 +291,7 @@ // The Stellaris ACK API definition. // //***************************************************************************** -#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_ACK) +#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK) //***************************************************************************** // @@ -350,8 +342,7 @@ // The Stellaris Motor Class Periodic Status API definitions. // //***************************************************************************** -#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | \ - CAN_API_MC_PSTAT) +#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT) #define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S)) #define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S)) #define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S)) @@ -372,39 +363,37 @@ // little-endian, therefore B0 is the least significant byte. // //***************************************************************************** -#define LM_PSTAT_END 0 -#define LM_PSTAT_VOLTOUT_B0 1 -#define LM_PSTAT_VOLTOUT_B1 2 -#define LM_PSTAT_VOLTBUS_B0 3 -#define LM_PSTAT_VOLTBUS_B1 4 -#define LM_PSTAT_CURRENT_B0 5 -#define LM_PSTAT_CURRENT_B1 6 -#define LM_PSTAT_TEMP_B0 7 -#define LM_PSTAT_TEMP_B1 8 -#define LM_PSTAT_POS_B0 9 -#define LM_PSTAT_POS_B1 10 -#define LM_PSTAT_POS_B2 11 -#define LM_PSTAT_POS_B3 12 -#define LM_PSTAT_SPD_B0 13 -#define LM_PSTAT_SPD_B1 14 -#define LM_PSTAT_SPD_B2 15 -#define LM_PSTAT_SPD_B3 16 -#define LM_PSTAT_LIMIT_NCLR 17 -#define LM_PSTAT_LIMIT_CLR 18 -#define LM_PSTAT_FAULT 19 -#define LM_PSTAT_STKY_FLT_NCLR 20 -#define LM_PSTAT_STKY_FLT_CLR 21 -#define LM_PSTAT_VOUT_B0 22 -#define LM_PSTAT_VOUT_B1 23 -#define LM_PSTAT_FLT_COUNT_CURRENT \ - 24 -#define LM_PSTAT_FLT_COUNT_TEMP 25 -#define LM_PSTAT_FLT_COUNT_VOLTBUS \ - 26 -#define LM_PSTAT_FLT_COUNT_GATE 27 -#define LM_PSTAT_FLT_COUNT_COMM 28 -#define LM_PSTAT_CANSTS 29 -#define LM_PSTAT_CANERR_B0 30 -#define LM_PSTAT_CANERR_B1 31 +#define LM_PSTAT_END 0 +#define LM_PSTAT_VOLTOUT_B0 1 +#define LM_PSTAT_VOLTOUT_B1 2 +#define LM_PSTAT_VOLTBUS_B0 3 +#define LM_PSTAT_VOLTBUS_B1 4 +#define LM_PSTAT_CURRENT_B0 5 +#define LM_PSTAT_CURRENT_B1 6 +#define LM_PSTAT_TEMP_B0 7 +#define LM_PSTAT_TEMP_B1 8 +#define LM_PSTAT_POS_B0 9 +#define LM_PSTAT_POS_B1 10 +#define LM_PSTAT_POS_B2 11 +#define LM_PSTAT_POS_B3 12 +#define LM_PSTAT_SPD_B0 13 +#define LM_PSTAT_SPD_B1 14 +#define LM_PSTAT_SPD_B2 15 +#define LM_PSTAT_SPD_B3 16 +#define LM_PSTAT_LIMIT_NCLR 17 +#define LM_PSTAT_LIMIT_CLR 18 +#define LM_PSTAT_FAULT 19 +#define LM_PSTAT_STKY_FLT_NCLR 20 +#define LM_PSTAT_STKY_FLT_CLR 21 +#define LM_PSTAT_VOUT_B0 22 +#define LM_PSTAT_VOUT_B1 23 +#define LM_PSTAT_FLT_COUNT_CURRENT 24 +#define LM_PSTAT_FLT_COUNT_TEMP 25 +#define LM_PSTAT_FLT_COUNT_VOLTBUS 26 +#define LM_PSTAT_FLT_COUNT_GATE 27 +#define LM_PSTAT_FLT_COUNT_COMM 28 +#define LM_PSTAT_CANSTS 29 +#define LM_PSTAT_CANERR_B0 30 +#define LM_PSTAT_CANERR_B1 31 #endif // __CAN_PROTO_H__ diff --git a/puma_motor_driver/include/puma_motor_driver/driver.hpp b/puma_motor_driver/include/puma_motor_driver/driver.hpp index 2ae4de8..0a237e4 100644 --- a/puma_motor_driver/include/puma_motor_driver/driver.hpp +++ b/puma_motor_driver/include/puma_motor_driver/driver.hpp @@ -40,10 +40,11 @@ namespace puma_motor_driver class Driver { public: - Driver(const std::shared_ptr interface, + Driver( + const std::shared_ptr interface, std::shared_ptr nh, - const uint8_t& device_number, - const std::string& device_name); + const uint8_t & device_number, + const std::string & device_name); void processMessage(const can_msgs::msg::Frame::SharedPtr received_msg); @@ -394,21 +395,21 @@ class Driver * * @return pointer to raw 4 bytes of the P gain response. */ - uint8_t* getRawP(); + uint8_t * getRawP(); /** * Process the last received I gain * for the current control mode. * * @return pointer to raw 4 bytes of the I gain response. */ - uint8_t* getRawI(); + uint8_t * getRawI(); /** * Process the last received I gain * for the current control mode. * * @return pointer to raw 4 bytes of the I gain response. */ - uint8_t* getRawD(); + uint8_t * getRawD(); /** * Process the last received set-point response * in voltage open-loop control. @@ -438,11 +439,9 @@ class Driver */ double statusPositionGet(); + std::string deviceName() const {return device_name_;} - - std::string deviceName() const { return device_name_; } - - uint8_t deviceNumber() const { return device_number_; } + uint8_t deviceNumber() const {return device_number_;} // Only used internally but is used for testing. struct Field @@ -452,16 +451,15 @@ class Driver float interpretFixed8x8() { - return *(reinterpret_cast(data)) / static_cast(1<<8); + return *(reinterpret_cast(data)) / static_cast(1 << 8); } double interpretFixed16x16() { - return *(reinterpret_cast(data)) / static_cast(1<<16); + return *(reinterpret_cast(data)) / static_cast(1 << 16); } }; - private: std::shared_ptr interface_; std::shared_ptr nh_; @@ -497,7 +495,7 @@ class Driver * * @return boolean if received is equal to expected. */ - bool verifyRaw16x16(const uint8_t* received, const double expected); + bool verifyRaw16x16(const uint8_t * received, const double expected); /** * Comparing the raw bytes of the 8x8 fixed-point numbers @@ -505,7 +503,7 @@ class Driver * * @return boolean if received is equal to expected. */ - bool verifyRaw8x8(const uint8_t* received, const float expected); + bool verifyRaw8x8(const uint8_t * received, const float expected); Field voltage_fields_[4]; Field spd_fields_[7]; @@ -515,13 +513,13 @@ class Driver Field status_fields_[15]; Field cfg_fields_[15]; - Field* voltageFieldForMessage(uint32_t api); - Field* spdFieldForMessage(uint32_t api); - Field* vcompFieldForMessage(uint32_t api); - Field* posFieldForMessage(uint32_t api); - Field* ictrlFieldForMessage(uint32_t api); - Field* statusFieldForMessage(uint32_t api); - Field* cfgFieldForMessage(uint32_t api); + Field * voltageFieldForMessage(uint32_t api); + Field * spdFieldForMessage(uint32_t api); + Field * vcompFieldForMessage(uint32_t api); + Field * posFieldForMessage(uint32_t api); + Field * ictrlFieldForMessage(uint32_t api); + Field * statusFieldForMessage(uint32_t api); + Field * cfgFieldForMessage(uint32_t api); }; } // namespace puma_motor_driver diff --git a/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp b/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp index 3da5443..14c10de 100644 --- a/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp +++ b/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp @@ -38,33 +38,35 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "puma_motor_driver/driver.hpp" // #include "puma_motor_driver/diagnostic_updater.hpp" -namespace FeedbackBit{ - enum - { - DutyCycle, - Current, - Position, - Speed, - Setpoint, - Count, - }; +namespace FeedbackBit +{ +enum +{ + DutyCycle, + Current, + Position, + Speed, + Setpoint, + Count, }; +} -namespace StatusBit{ - enum - { - BusVoltage, - OutVoltage, - AnalogInput, - Temperature, - Mode, - Fault, - Count, - }; +namespace StatusBit +{ +enum +{ + BusVoltage, + OutVoltage, + AnalogInput, + Temperature, + Mode, + Fault, + Count, }; +} class MultiPumaNode -: public rclcpp::Node + : public rclcpp::Node { public: MultiPumaNode(const std::string node_name); diff --git a/puma_motor_driver/src/driver.cpp b/puma_motor_driver/src/driver.cpp index d661582..8245130 100644 --- a/puma_motor_driver/src/driver.cpp +++ b/puma_motor_driver/src/driver.cpp @@ -36,75 +36,78 @@ namespace puma_motor_driver namespace ConfigurationStates { - enum ConfigurationState - { - Unknown = -1, - Initializing, - PowerFlag, - EncoderPosRef, - EncoderSpdRef, - EncoderCounts, - ClosedLoop, - ControlMode, - PGain, - IGain, - DGain, - VerifiedParameters, - Configured - }; +enum ConfigurationState +{ + Unknown = -1, + Initializing, + PowerFlag, + EncoderPosRef, + EncoderSpdRef, + EncoderCounts, + ClosedLoop, + ControlMode, + PGain, + IGain, + DGain, + VerifiedParameters, + Configured +}; } // namespace ConfigurationStates typedef ConfigurationStates::ConfigurationState ConfigurationState; -Driver::Driver(const std::shared_ptr interface, +Driver::Driver( + const std::shared_ptr interface, std::shared_ptr nh, - const uint8_t& device_number, - const std::string& device_name) - : interface_(interface), nh_(nh), device_number_(device_number), device_name_(device_name), - configured_(false), state_(ConfigurationState::Initializing), control_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), - gain_p_(1), gain_i_(0), gain_d_(0), encoder_cpr_(1), gear_ratio_(1) - { - } + const uint8_t & device_number, + const std::string & device_name) +: interface_(interface), + nh_(nh), + device_number_(device_number), + device_name_(device_name), + configured_(false), + state_(ConfigurationState::Initializing), + control_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), + gain_p_(1), + gain_i_(0), + gain_d_(0), + encoder_cpr_(1), + gear_ratio_(1) +{ +} void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) { // If it's not our message, jump out. - if (getDeviceNumber(*received_msg) != device_number_) return; + if (getDeviceNumber(*received_msg) != device_number_) { + return; + } // If there's no data then this is a request message, jump out. - if (received_msg->dlc == 0) return; + if (received_msg->dlc == 0) { + return; + } - Field* field = nullptr; + Field * field = nullptr; uint32_t received_api = getApi(*received_msg); - if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) - { + if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) { field = cfgFieldForMessage(received_api); - } - else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) - { + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) { field = statusFieldForMessage(received_api); - } - else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) - { + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) { field = ictrlFieldForMessage(received_api); - } - else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) - { + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) { field = posFieldForMessage(received_api); - } - else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) - { + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) { field = vcompFieldForMessage(received_api); - } - else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) - { + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) { field = spdFieldForMessage(received_api); - } - else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) - { + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) { field = voltageFieldForMessage(received_api); } - if (!field) return; + if (!field) { + return; + } // Copy the received data and mark that field as received. std::copy(std::begin(received_msg->data), std::end(received_msg->data), std::begin(field->data)); @@ -113,7 +116,7 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg) double Driver::radPerSecToRpm() const { - return ((60 * gear_ratio_) / (2 * M_PI)); + return (60 * gear_ratio_) / (2 * M_PI); } void Driver::sendId(const uint32_t id) @@ -148,7 +151,7 @@ void Driver::sendFixed8x8(const uint32_t id, const float value) { can_msgs::msg::Frame msg = getMsg(id); msg.dlc = sizeof(int16_t); - int16_t output_value = static_cast(static_cast(1<<8) * value); + int16_t output_value = static_cast(static_cast(1 << 8) * value); uint8_t data[8] = {0}; std::memcpy(data, &output_value, sizeof(int16_t)); @@ -161,7 +164,7 @@ void Driver::sendFixed16x16(const uint32_t id, const double value) { can_msgs::msg::Frame msg = getMsg(id); msg.dlc = sizeof(int32_t); - int32_t output_value = static_cast(static_cast((1<<16) * value)); + int32_t output_value = static_cast(static_cast((1 << 16) * value)); uint8_t data[8] = {0}; std::memcpy(data, &output_value, sizeof(int32_t)); @@ -191,15 +194,13 @@ uint32_t Driver::getDeviceNumber(const can_msgs::msg::Frame msg) return msg.id & CAN_MSGID_DEVNO_M; } -bool Driver::verifyRaw16x16(const uint8_t* received, const double expected) +bool Driver::verifyRaw16x16(const uint8_t * received, const double expected) { uint8_t data[4]; - int32_t output_value = static_cast(static_cast((1<<16) * expected)); + int32_t output_value = static_cast(static_cast((1 << 16) * expected)); std::memcpy(data, &output_value, 4); - for (uint8_t i = 0; i < 4; i++) - { - if (*received != data[i]) - { + for (uint8_t i = 0; i < 4; i++) { + if (*received != data[i]) { return false; } received++; @@ -207,15 +208,13 @@ bool Driver::verifyRaw16x16(const uint8_t* received, const double expected) return true; } -bool Driver::verifyRaw8x8(const uint8_t* received, const float expected) +bool Driver::verifyRaw8x8(const uint8_t * received, const float expected) { uint8_t data[2]; - int32_t output_value = static_cast(static_cast((1<<8) * expected)); + int32_t output_value = static_cast(static_cast((1 << 8) * expected)); std::memcpy(data, &output_value, 2); - for (uint8_t i = 0; i < 2; i++) - { - if (*received != data[i]) - { + for (uint8_t i = 0; i < 2; i++) { + if (*received != data[i]) { return false; } received++; @@ -246,102 +245,89 @@ void Driver::commandSpeed(const double cmd) void Driver::verifyParams() { - switch (state_) - { + switch (state_) { case ConfigurationState::Initializing: - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): starting to verify parameters.", - device_name_.c_str(), device_number_); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): starting to verify parameters.", + device_name_.c_str(), device_number_); state_ = ConfigurationState::PowerFlag; break; case ConfigurationState::PowerFlag: - if (lastPower() == 0) - { + if (lastPower() == 0) { state_ = ConfigurationState::EncoderPosRef; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): cleared power flag.", device_name_.c_str(), device_number_); - } - else - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): cleared power flag.", + device_name_.c_str(), device_number_); + } else { sendId(LM_API_STATUS_POWER | device_number_); } break; case ConfigurationState::EncoderPosRef: - if (posEncoderRef() == LM_REF_ENCODER) - { + if (posEncoderRef() == LM_REF_ENCODER) { state_ = ConfigurationState::EncoderSpdRef; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): set position encoder reference.", - device_name_.c_str(), device_number_); - } - else - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set position encoder reference.", + device_name_.c_str(), device_number_); + } else { sendId(LM_API_POS_REF | device_number_); } break; case ConfigurationState::EncoderSpdRef: - if (spdEncoderRef() == LM_REF_QUAD_ENCODER) - { + if (spdEncoderRef() == LM_REF_QUAD_ENCODER) { state_ = ConfigurationState::EncoderCounts; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): set speed encoder reference.", - device_name_.c_str(), device_number_); - } - else - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set speed encoder reference.", + device_name_.c_str(), device_number_); + } else { sendId(LM_API_SPD_REF | device_number_); } break; case ConfigurationState::EncoderCounts: - if (encoderCounts() == encoder_cpr_) - { + if (encoderCounts() == encoder_cpr_) { state_ = ConfigurationState::ClosedLoop; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): set encoder counts to %i.", - device_name_.c_str(), device_number_, encoder_cpr_); - } - else - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set encoder counts to %i.", + device_name_.c_str(), device_number_, encoder_cpr_); + } else { sendId(LM_API_CFG_ENC_LINES | device_number_); } break; case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. - if (lastMode() == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) - { + if (lastMode() == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { state_ = ConfigurationState::ControlMode; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): entered a close-loop control mode.", - device_name_.c_str(), device_number_); - } - else - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): entered a close-loop control mode.", + device_name_.c_str(), device_number_); + } else { sendId(LM_API_STATUS_CMODE | device_number_); } break; case ConfigurationState::ControlMode: - if (lastMode() == control_mode_) - { - if (control_mode_ != clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) - { + if (lastMode() == control_mode_) { + if (control_mode_ != clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { state_ = ConfigurationState::PGain; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): was set to a close loop control mode.", - device_name_.c_str(), device_number_); - } - else - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): was set to a close loop control mode.", + device_name_.c_str(), device_number_); + } else { state_ = ConfigurationState::VerifiedParameters; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): was set to voltage control mode.", - device_name_.c_str(), device_number_); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): was set to voltage control mode.", + device_name_.c_str(), device_number_); } } break; case ConfigurationState::PGain: - if (verifyRaw16x16(getRawP(), gain_p_)) - { + if (verifyRaw16x16(getRawP(), gain_p_)) { state_ = ConfigurationState::IGain; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getP(), gain_p_); - } - else - { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getP(), gain_p_); - switch (control_mode_) - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getP(), gain_p_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getP(), gain_p_); + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_PC | device_number_); break; @@ -355,18 +341,16 @@ void Driver::verifyParams() } break; case ConfigurationState::IGain: - if (verifyRaw16x16(getRawI(), gain_i_)) - { + if (verifyRaw16x16(getRawI(), gain_i_)) { state_ = ConfigurationState::DGain; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getI(), gain_i_); - } - else - { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getI(), gain_i_); - switch (control_mode_) - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getI(), gain_i_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getI(), gain_i_); + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_IC | device_number_); break; @@ -380,18 +364,16 @@ void Driver::verifyParams() } break; case ConfigurationState::DGain: - if (verifyRaw16x16(getRawD(), gain_d_)) - { + if (verifyRaw16x16(getRawD(), gain_d_)) { state_ = ConfigurationState::VerifiedParameters; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getD(), gain_d_); - } - else - { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getD(), gain_d_); - switch (control_mode_) - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getD(), gain_d_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getD(), gain_d_); + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_DC | device_number_); break; @@ -405,9 +387,10 @@ void Driver::verifyParams() } break; } - if (state_ == ConfigurationState::VerifiedParameters) - { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): all parameters verified.", device_name_.c_str(), device_number_); + if (state_ == ConfigurationState::VerifiedParameters) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): all parameters verified.", + device_name_.c_str(), device_number_); configured_ = true; state_ = ConfigurationState::Configured; } @@ -415,8 +398,7 @@ void Driver::verifyParams() void Driver::configureParams() { - switch (state_) - { + switch (state_) { case ConfigurationState::PowerFlag: sendUint8((LM_API_STATUS_POWER | device_number_), 1); break; @@ -434,8 +416,7 @@ void Driver::configureParams() sendId(LM_API_SPD_EN | device_number_); break; case ConfigurationState::ControlMode: - switch (control_mode_) - { + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: sendId(LM_API_VOLT_EN | device_number_); break; @@ -452,46 +433,43 @@ void Driver::configureParams() break; case ConfigurationState::PGain: // Set P - switch (control_mode_) - { + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendFixed16x16((LM_API_ICTRL_PC | device_number_), gain_p_); + sendFixed16x16((LM_API_ICTRL_PC | device_number_), gain_p_); break; case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendFixed16x16((LM_API_POS_PC | device_number_), gain_p_); + sendFixed16x16((LM_API_POS_PC | device_number_), gain_p_); break; case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendFixed16x16((LM_API_SPD_PC | device_number_), gain_p_); + sendFixed16x16((LM_API_SPD_PC | device_number_), gain_p_); break; } break; case ConfigurationState::IGain: // Set I - switch (control_mode_) - { + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendFixed16x16((LM_API_ICTRL_IC | device_number_), gain_i_); + sendFixed16x16((LM_API_ICTRL_IC | device_number_), gain_i_); break; case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendFixed16x16((LM_API_POS_IC | device_number_), gain_i_); + sendFixed16x16((LM_API_POS_IC | device_number_), gain_i_); break; case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendFixed16x16((LM_API_SPD_IC | device_number_), gain_i_); + sendFixed16x16((LM_API_SPD_IC | device_number_), gain_i_); break; } break; case ConfigurationState::DGain: // Set D - switch (control_mode_) - { + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendFixed16x16((LM_API_ICTRL_DC | device_number_), gain_d_); + sendFixed16x16((LM_API_ICTRL_DC | device_number_), gain_d_); break; case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendFixed16x16((LM_API_POS_DC | device_number_), gain_d_); + sendFixed16x16((LM_API_POS_DC | device_number_), gain_d_); break; case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendFixed16x16((LM_API_SPD_DC | device_number_), gain_d_); + sendFixed16x16((LM_API_SPD_DC | device_number_), gain_d_); break; } break; @@ -509,47 +487,41 @@ void Driver::setGains(const double p, const double i, const double d) gain_i_ = i; gain_d_ = d; - if (configured_) - { + if (configured_) { updateGains(); } } void Driver::setMode(const uint8_t mode) { - if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) - { + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { control_mode_ = mode; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): mode set to voltage control.", device_name_.c_str(), device_number_); - if (configured_) - { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to voltage control.", + device_name_.c_str(), device_number_); + if (configured_) { resetConfiguration(); } - } - else - { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): Close loop modes need PID gains.", - device_name_.c_str(), device_number_); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): Close loop modes need PID gains.", + device_name_.c_str(), device_number_); } } void Driver::setMode(const uint8_t mode, const double p, const double i, const double d) { - if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) - { + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { control_mode_ = mode; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Puma Motor Controller on %s (%i): mode set to voltage control but PID gains are not needed.", - device_name_.c_str(), device_number_); - if (configured_) - { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to voltage control but PID gains are not needed.", + device_name_.c_str(), device_number_); + if (configured_) { resetConfiguration(); } - } - else - { + } else { control_mode_ = mode; - if (configured_) - { + if (configured_) { resetConfiguration(); } setGains(p, i, d); @@ -574,16 +546,16 @@ void Driver::clearMsgCache() void Driver::requestStatusMessages() { - sendId(LM_API_STATUS_POWER | device_number_); + sendId(LM_API_STATUS_POWER | device_number_); } void Driver::requestFeedbackMessages() { sendId(LM_API_STATUS_VOLTOUT | device_number_); sendId(LM_API_STATUS_CURRENT | device_number_); - sendId(LM_API_STATUS_POS | device_number_); - sendId(LM_API_STATUS_SPD | device_number_); - sendId(LM_API_SPD_SET | device_number_); + sendId(LM_API_STATUS_POS | device_number_); + sendId(LM_API_STATUS_SPD | device_number_); + sendId(LM_API_SPD_SET | device_number_); } void Driver::requestFeedbackDutyCycle() { @@ -612,8 +584,7 @@ void Driver::requestFeedbackPowerState() void Driver::requestFeedbackSetpoint() { - switch (control_mode_) - { + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: sendId(LM_API_ICTRL_SET | device_number_); break; @@ -626,7 +597,7 @@ void Driver::requestFeedbackSetpoint() case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: sendId(LM_API_VOLT_SET | device_number_); break; - }; + } } void Driver::resetConfiguration() @@ -643,74 +614,73 @@ void Driver::updateGains() bool Driver::receivedDutyCycle() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); return field->received; } bool Driver::receivedBusVoltage() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); return field->received; } bool Driver::receivedCurrent() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); return field->received; } bool Driver::receivedPosition() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); return field->received; } bool Driver::receivedSpeed() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); return field->received; } bool Driver::receivedFault() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); return field->received; } bool Driver::receivedPower() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); return field->received; } bool Driver::receivedMode() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); return field->received; } bool Driver::receivedOutVoltage() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); return field->received; } bool Driver::receivedTemperature() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); return field->received; } bool Driver::receivedAnalogInput() { - Field* field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); return field->received; } bool Driver::receivedSetpoint() { - switch (control_mode_) - { + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: return receivedCurrentSetpoint(); break; @@ -731,109 +701,108 @@ bool Driver::receivedSetpoint() bool Driver::receivedSpeedSetpoint() { - Field* field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); return field->received; } bool Driver::receivedDutyCycleSetpoint() { - Field* field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); return field->received; } bool Driver::receivedCurrentSetpoint() { - Field* field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); return field->received; } bool Driver::receivedPositionSetpoint() { - Field* field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); return field->received; } float Driver::lastDutyCycle() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); field->received = false; - return (field->interpretFixed8x8() / 128.0); + return field->interpretFixed8x8() / 128.0; } float Driver::lastBusVoltage() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); field->received = false; return field->interpretFixed8x8(); } float Driver::lastCurrent() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); field->received = false; return field->interpretFixed8x8(); } double Driver::lastPosition() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); field->received = false; - return (field->interpretFixed16x16() * ((2 * M_PI) / gear_ratio_)); // Convert rev to rad + return field->interpretFixed16x16() * ((2 * M_PI) / gear_ratio_); // Convert rev to rad } double Driver::lastSpeed() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); field->received = false; - return (field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60))); // Convert RPM to rad/s + return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s } uint8_t Driver::lastFault() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); field->received = false; return field->data[0]; } uint8_t Driver::lastPower() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); field->received = false; return field->data[0]; } uint8_t Driver::lastMode() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); field->received = false; return field->data[0]; } float Driver::lastOutVoltage() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); field->received = false; return field->interpretFixed8x8(); } float Driver::lastTemperature() { - Field* field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); field->received = false; return field->interpretFixed8x8(); } float Driver::lastAnalogInput() { - Field* field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); field->received = false; return field->interpretFixed8x8(); } double Driver::lastSetpoint() { - switch (control_mode_) - { + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: return statusCurrentGet(); break; @@ -853,55 +822,54 @@ double Driver::lastSetpoint() } double Driver::statusSpeedGet() { - Field* field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); field->received = false; - return (field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60))); // Convert RPM to rad/s + return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s } float Driver::statusDutyCycleGet() { - Field* field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); field->received = false; - return (field->interpretFixed8x8() / 128.0); + return field->interpretFixed8x8() / 128.0; } float Driver::statusCurrentGet() { - Field* field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); field->received = false; return field->interpretFixed8x8(); } double Driver::statusPositionGet() { - Field* field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); field->received = false; - return (field->interpretFixed16x16() * (( 2 * M_PI) / gear_ratio_)); // Convert rev to rad + return field->interpretFixed16x16() * (( 2 * M_PI) / gear_ratio_); // Convert rev to rad } uint8_t Driver::posEncoderRef() { - Field* field = posFieldForMessage(getApi(getMsg(LM_API_POS_REF))); + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_REF))); return field->data[0]; } uint8_t Driver::spdEncoderRef() { - Field* field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_REF))); + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_REF))); return field->data[0]; } uint16_t Driver::encoderCounts() { - Field* field = cfgFieldForMessage(getApi(getMsg(LM_API_CFG_ENC_LINES))); - return (static_cast(field->data[0]) | static_cast(field->data[1] << 8)); + Field * field = cfgFieldForMessage(getApi(getMsg(LM_API_CFG_ENC_LINES))); + return static_cast(field->data[0]) | static_cast(field->data[1] << 8); } double Driver::getP() { - Field* field; - switch (control_mode_) - { + Field * field; + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); break; @@ -911,15 +879,14 @@ double Driver::getP() case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); break; - }; + } return field->interpretFixed16x16(); } double Driver::getI() { - Field* field; - switch (control_mode_) - { + Field * field; + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); break; @@ -929,15 +896,14 @@ double Driver::getI() case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); break; - }; + } return field->interpretFixed16x16(); } double Driver::getD() { - Field* field; - switch (control_mode_) - { + Field * field; + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); break; @@ -947,15 +913,14 @@ double Driver::getD() case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); break; - }; + } return field->interpretFixed16x16(); } -uint8_t* Driver::getRawP() +uint8_t * Driver::getRawP() { - Field* field; - switch (control_mode_) - { + Field * field; + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); break; @@ -965,15 +930,14 @@ uint8_t* Driver::getRawP() case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); break; - }; + } return field->data; } -uint8_t* Driver::getRawI() +uint8_t * Driver::getRawI() { - Field* field; - switch (control_mode_) - { + Field * field; + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); break; @@ -983,15 +947,14 @@ uint8_t* Driver::getRawI() case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); break; - }; + } return field->data; } -uint8_t* Driver::getRawD() +uint8_t * Driver::getRawD() { - Field* field; - switch (control_mode_) - { + Field * field; + switch (control_mode_) { case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); break; @@ -1001,47 +964,47 @@ uint8_t* Driver::getRawD() case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); break; - }; + } return field->data; } -Driver::Field* Driver::voltageFieldForMessage(uint32_t api) +Driver::Field * Driver::voltageFieldForMessage(uint32_t api) { uint32_t voltage_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &voltage_fields_[voltage_field_index]; } -Driver::Field* Driver::spdFieldForMessage(uint32_t api) +Driver::Field * Driver::spdFieldForMessage(uint32_t api) { uint32_t spd_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &spd_fields_[spd_field_index]; } -Driver::Field* Driver::vcompFieldForMessage(uint32_t api) +Driver::Field * Driver::vcompFieldForMessage(uint32_t api) { uint32_t vcomp_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &vcomp_fields_[vcomp_field_index]; } -Driver::Field* Driver::posFieldForMessage(uint32_t api) +Driver::Field * Driver::posFieldForMessage(uint32_t api) { uint32_t pos_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &pos_fields_[pos_field_index]; } -Driver::Field* Driver::ictrlFieldForMessage(uint32_t api) +Driver::Field * Driver::ictrlFieldForMessage(uint32_t api) { uint32_t ictrl_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &ictrl_fields_[ictrl_field_index]; } -Driver::Field* Driver::statusFieldForMessage(uint32_t api) +Driver::Field * Driver::statusFieldForMessage(uint32_t api) { uint32_t status_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &status_fields_[status_field_index]; } -Driver::Field* Driver::cfgFieldForMessage(uint32_t api) +Driver::Field * Driver::cfgFieldForMessage(uint32_t api) { uint32_t cfg_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; return &cfg_fields_[cfg_field_index]; diff --git a/puma_motor_driver/src/multi_puma_node.cpp b/puma_motor_driver/src/multi_puma_node.cpp index 837fc17..2ba2039 100644 --- a/puma_motor_driver/src/multi_puma_node.cpp +++ b/puma_motor_driver/src/multi_puma_node.cpp @@ -23,11 +23,11 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI */ #include "puma_motor_driver/multi_puma_node.hpp" -MultiPumaNode::MultiPumaNode(const std::string node_name) : - Node(node_name), - active_(false), - status_count_(0), - desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) +MultiPumaNode::MultiPumaNode(const std::string node_name) +:Node(node_name), + active_(false), + Status_count_(0), + desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { // Parameters this->declare_parameter("canbus_dev", "vcan0"); @@ -65,7 +65,8 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) : ); // Validate Parameters - if (joint_names_.size() != joint_can_ids_.size() || joint_names_.size() != joint_directions_.size()) + if (joint_names_.size() != joint_can_ids_.size() || + joint_names_.size() != joint_directions_.size()) { RCLCPP_ERROR( this->get_logger(), @@ -90,10 +91,10 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) : node_handle_ = std::shared_ptr(this, [](rclcpp::Node *){}); // Socket - interface_.reset(new clearpath_ros2_socketcan_interface::SocketCANInterface(canbus_dev_, node_handle_)); + interface_.reset(new clearpath_ros2_socketcan_interface::SocketCANInterface( + canbus_dev_, node_handle_)); - for (uint8_t i = 0; i < joint_names_.size(); i++) - { + for (uint8_t i = 0; i < joint_names_.size(); i++) { drivers_.push_back(puma_motor_driver::Driver( interface_, node_handle_, @@ -107,8 +108,7 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) : status_msg_.drivers.resize(drivers_.size()); uint8_t i = 0; - for (auto &driver : drivers_) - { + for (auto & driver : drivers_) { driver.clearMsgCache(); driver.setEncoderCPR(encoder_cpr_); driver.setGearRatio(gear_ratio_ * joint_directions_[i]); @@ -117,15 +117,14 @@ MultiPumaNode::MultiPumaNode(const std::string node_name) : } run_timer_ = this->create_wall_timer( - std::chrono::milliseconds(1000/freq_), std::bind(&MultiPumaNode::run, this)); + std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); } bool MultiPumaNode::getFeedback() { // Check All Fields Received uint8_t received = 0; - for (auto& driver : drivers_) - { + for (auto & driver : drivers_) { received |= driver.receivedDutyCycle() << FeedbackBit::DutyCycle; received |= driver.receivedCurrent() << FeedbackBit::Current; received |= driver.receivedPosition() << FeedbackBit::Position; @@ -133,15 +132,13 @@ bool MultiPumaNode::getFeedback() received |= driver.receivedSetpoint() << FeedbackBit::Setpoint; } - if (received != (1 << FeedbackBit::Count) - 1) - { + if (received != (1 << FeedbackBit::Count) - 1) { return false; } uint8_t feedback_index = 0; - for (auto& driver : drivers_) - { - clearpath_motor_msgs::msg::PumaFeedback* f = &feedback_msg_.drivers_feedback[feedback_index]; + for (auto & driver : drivers_) { + clearpath_motor_msgs::msg::PumaFeedback * f = &feedback_msg_.drivers_feedback[feedback_index]; f->device_number = driver.deviceNumber(); f->device_name = driver.deviceName(); f->duty_cycle = driver.lastDutyCycle(); @@ -162,8 +159,7 @@ bool MultiPumaNode::getStatus() uint8_t status_index = 0; uint8_t received_fields = 0; uint8_t received_status = 0; - for (auto& driver : drivers_) - { + for (auto & driver : drivers_) { received_fields |= driver.receivedBusVoltage() << StatusBit::BusVoltage; received_fields |= driver.receivedOutVoltage() << StatusBit::OutVoltage; received_fields |= driver.receivedAnalogInput() << StatusBit::AnalogInput; @@ -171,28 +167,23 @@ bool MultiPumaNode::getStatus() received_fields |= driver.receivedTemperature() << StatusBit::Temperature; received_fields |= driver.receivedMode() << StatusBit::Mode; received_fields |= driver.receivedFault() << StatusBit::Fault; - if (received_fields != (1 << StatusBit::Count) - 1) - { + if (received_fields != (1 << StatusBit::Count) - 1) { RCLCPP_DEBUG(this->get_logger(), "Received Status Fields %x", received_fields); - } - else - { + } else { received_status |= 1 << status_index; } status_index++; } - if (received_status != (1 << status_index) - 1) - { + if (received_status != (1 << status_index) - 1) { RCLCPP_DEBUG(this->get_logger(), "Received Status %x", received_status); return false; } // Prepare output status message to ROS. status_index = 0; - for (auto& driver : drivers_) - { - clearpath_motor_msgs::msg::PumaStatus* s = &status_msg_.drivers[status_index]; + for (auto & driver : drivers_) { + clearpath_motor_msgs::msg::PumaStatus * s = &status_msg_.drivers[status_index]; s->device_number = driver.deviceNumber(); s->device_name = driver.deviceName(); s->bus_voltage = driver.lastBusVoltage(); @@ -210,36 +201,27 @@ bool MultiPumaNode::getStatus() void MultiPumaNode::publishFeedback() { - if (getFeedback()) - { + if (getFeedback()) { feedback_pub_->publish(feedback_msg_); } } void MultiPumaNode::publishStatus() { - if (getStatus()) - { + if (getStatus()) { status_pub_->publish(status_msg_); } } void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { - if (active_) - { - for (auto& driver : drivers_) - { - for (int i = 0; i (msg->name.size()); i++) - { - if (driver.deviceName() == msg->name[i]) - { - if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) - { + if (active_) { + for (auto & driver : drivers_) { + for (int i = 0; i < static_cast(msg->name.size()); i++) { + if (driver.deviceName() == msg->name[i]) { + if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { driver.commandDutyCycle(msg->velocity[i]); - } - else if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) - { + } else if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { driver.commandSpeed(msg->velocity[i]); } } @@ -250,10 +232,8 @@ void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr ms bool MultiPumaNode::areAllActive() { - for (auto& driver : drivers_) - { - if (!driver.isConfigured()) - { + for (auto & driver : drivers_) { + if (!driver.isConfigured()) { return false; } } @@ -262,64 +242,52 @@ bool MultiPumaNode::areAllActive() void MultiPumaNode::run() { - if (active_) - { + if (active_) { // Checks to see if power flag has been reset for each driver - for (auto& driver : drivers_) - { - if (driver.lastPower() != 0) - { + for (auto & driver : drivers_) { + if (driver.lastPower() != 0) { active_ = false; - RCLCPP_WARN(this->get_logger(), "Power reset detected on device ID %d, will reconfigure all drivers.", driver.deviceNumber()); - for (auto& driver : drivers_) - { + RCLCPP_WARN(this->get_logger(), + "Power reset detected on device ID %d, will reconfigure all drivers.", + driver.deviceNumber()); + for (auto & driver : drivers_) { driver.resetConfiguration(); } } } // Queue data requests for the drivers in order to assemble an amalgamated status message. - for (auto& driver : drivers_) - { + for (auto & driver : drivers_) { driver.requestStatusMessages(); driver.requestFeedbackSetpoint(); } - } - else - { + } else { // Set parameters for each driver. - for (auto& driver : drivers_) - { + for (auto & driver : drivers_) { driver.configureParams(); } } // Process all received messages through the connected driver instances. - while (interface_->recv(recv_msg_)) - { - for (auto& driver : drivers_) - { + while (interface_->recv(recv_msg_)) { + for (auto & driver : drivers_) { driver.processMessage(recv_msg_); } } // Check parameters of each driver instance. - if (!active_) - { - for (auto& driver : drivers_) - { + if (!active_) { + for (auto & driver : drivers_) { driver.verifyParams(); } } // Verify that the all drivers are configured. - if (areAllActive() == true && active_ == false) - { + if (areAllActive() == true && active_ == false) { active_ = true; RCLCPP_INFO(this->get_logger(), "All controllers active."); } // Broadcast feedback and status messages - if (active_) - { + if (active_) { publishFeedback(); publishStatus(); } From 0dbcfa4ea7465dc99f68e2ea9dd3134333494459 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Wed, 6 Nov 2024 15:16:48 -0500 Subject: [PATCH 2/7] Accidental caps --- puma_motor_driver/src/multi_puma_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/puma_motor_driver/src/multi_puma_node.cpp b/puma_motor_driver/src/multi_puma_node.cpp index 2ba2039..4a48c15 100644 --- a/puma_motor_driver/src/multi_puma_node.cpp +++ b/puma_motor_driver/src/multi_puma_node.cpp @@ -26,7 +26,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI MultiPumaNode::MultiPumaNode(const std::string node_name) :Node(node_name), active_(false), - Status_count_(0), + status_count_(0), desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { // Parameters From ce061c623a5aa68efc87013cb2ff1f8ca4d9671d Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Wed, 6 Nov 2024 15:34:41 -0500 Subject: [PATCH 3/7] Add CI --- .github/workflows/ci.yml | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 .github/workflows/ci.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..48b9715 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,38 @@ +name: clearpath_motor_drivers_ci + +on: + push: + pull_request: + schedule: + - cron: "0 0 * * *" # every day at midnight + +jobs: + build_and_test: + name: jazzy + strategy: + matrix: + env: + - {ROS_DISTRO: jazzy, ROS_REPO: testing} + - {ROS_DISTRO: jazzy, ROS_REPO: main} + fail-fast: false + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v3 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} + + clearpath_motor_drivers_src_ci: + name: Jazzy Clearpath Source + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + - uses: ros-tooling/action-ros-ci@v0.3 + id: action_ros_ci_step + with: + target-ros2-distro: jazzy + package-name: | + puma_motor_driver + vcs-repo-file-url: dependencies.repos From 7481cb1cfaadf551f4464a70f496c8bb914f4a6f Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 7 Nov 2024 12:07:46 -0500 Subject: [PATCH 4/7] Remove repos file from CI --- .github/workflows/ci.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 48b9715..1448c02 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -35,4 +35,3 @@ jobs: target-ros2-distro: jazzy package-name: | puma_motor_driver - vcs-repo-file-url: dependencies.repos From ab8f8ec4b7258f475beec609b55d98a6dfe14ba2 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 7 Nov 2024 16:09:15 -0500 Subject: [PATCH 5/7] Add missing dependency on can_msgs --- puma_motor_driver/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/puma_motor_driver/package.xml b/puma_motor_driver/package.xml index 07d1de1..d0108d0 100644 --- a/puma_motor_driver/package.xml +++ b/puma_motor_driver/package.xml @@ -9,9 +9,10 @@ ament_cmake + can_msgs clearpath_motor_msgs - rclcpp clearpath_ros2_socketcan_interface + rclcpp sensor_msgs std_msgs From 036dd0a1d53fa4829ba32f6139666b1d63451d11 Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 7 Nov 2024 16:15:15 -0500 Subject: [PATCH 6/7] Add repo dependency on clearpath_msgs for source CI --- .github/workflows/ci.yml | 1 + dependencies.repos | 5 +++++ 2 files changed, 6 insertions(+) create mode 100644 dependencies.repos diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1448c02..48b9715 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -35,3 +35,4 @@ jobs: target-ros2-distro: jazzy package-name: | puma_motor_driver + vcs-repo-file-url: dependencies.repos diff --git a/dependencies.repos b/dependencies.repos new file mode 100644 index 0000000..9f9e1f1 --- /dev/null +++ b/dependencies.repos @@ -0,0 +1,5 @@ +repositories: + clearpath_msgs: + type: git + url: https://github.com/clearpathrobotics/clearpath_msgs.git + version: jazzy-2.0RC From 939bf5676061b597bead7bf784a129128710e49a Mon Sep 17 00:00:00 2001 From: Chris Iverach-Brereton Date: Thu, 7 Nov 2024 16:23:05 -0500 Subject: [PATCH 7/7] Add clearpath_ros2_socketcan_interface dependency repo --- dependencies.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dependencies.repos b/dependencies.repos index 9f9e1f1..b2f7565 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -3,3 +3,7 @@ repositories: type: git url: https://github.com/clearpathrobotics/clearpath_msgs.git version: jazzy-2.0RC + clearpath_ros2_socketcan_interface: + type: git + url: https://github.com/clearpathrobotics/clearpath_ros2_socketcan_interface.git + version: jazzy-2.0RC \ No newline at end of file