Skip to content

Commit

Permalink
Update alignment.
Browse files Browse the repository at this point in the history
  • Loading branch information
@civerachb-cpr
civerachb-cpr committed Nov 8, 2024
1 parent 5626796 commit 1e1583a
Show file tree
Hide file tree
Showing 6 changed files with 267 additions and 262 deletions.
5 changes: 5 additions & 0 deletions puma_motor_driver/.clang-format
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,11 @@ AccessModifierOffset: -2
ConstructorInitializerIndentWidth: 2
AlignEscapedNewlinesLeft: false
AlignTrailingComments: true
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: AcrossEmptyLines
AlignConsecutiveDeclarations: AcrossEmptyLines
AlignConsecutiveMacros: AcrossEmptyLines
AlignEscapedNewlines: LeftWithLastLine
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
Expand Down
300 changes: 150 additions & 150 deletions puma_motor_driver/include/puma_motor_driver/can_proto.hpp
View file

Large diffs are not rendered by default.

62 changes: 31 additions & 31 deletions puma_motor_driver/include/puma_motor_driver/driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ class Driver
Driver(const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
std::shared_ptr<rclcpp::Node> nh, const uint8_t& device_number, const std::string& device_name);

void processMessage(const can_msgs::msg::Frame::SharedPtr received_msg);
void processMessage(const can_msgs::msg::Frame::SharedPtr received_msg);

double radPerSecToRpm() const;

Expand Down Expand Up @@ -433,7 +433,7 @@ class Driver
*
* @return value of the set-point response.
*/
double statusPositionGet();
double statusPositionGet();

std::string deviceName() const
{
Expand All @@ -449,9 +449,9 @@ class Driver
struct Field
{
uint8_t data[4];
bool received;
bool received;

float interpretFixed8x8()
float interpretFixed8x8()
{
return *(reinterpret_cast<int16_t*>(data)) / static_cast<float>(1 << 8);
}
Expand All @@ -464,32 +464,32 @@ class Driver

private:
std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
std::shared_ptr<rclcpp::Node> nh_;
uint8_t device_number_;
std::string device_name_;
std::shared_ptr<rclcpp::Node> nh_;
uint8_t device_number_;
std::string device_name_;

bool configured_;
uint8_t state_;
bool configured_;
uint8_t state_;

uint8_t control_mode_;
double gain_p_;
double gain_i_;
double gain_d_;
uint16_t encoder_cpr_;
float gear_ratio_;
uint8_t control_mode_;
double gain_p_;
double gain_i_;
double gain_d_;
uint16_t encoder_cpr_;
float gear_ratio_;

/**
* Helpers to generate data for CAN messages.
*/
can_msgs::msg::Frame::SharedPtr can_msg_;
void sendId(const uint32_t id);
void sendUint8(const uint32_t id, const uint8_t value);
void sendUint16(const uint32_t id, const uint16_t value);
void sendFixed8x8(const uint32_t id, const float value);
void sendFixed16x16(const uint32_t id, const double value);
can_msgs::msg::Frame getMsg(const uint32_t id);
uint32_t getApi(const can_msgs::msg::Frame msg);
uint32_t getDeviceNumber(const can_msgs::msg::Frame msg);
void sendId(const uint32_t id);
void sendUint8(const uint32_t id, const uint8_t value);
void sendUint16(const uint32_t id, const uint16_t value);
void sendFixed8x8(const uint32_t id, const float value);
void sendFixed16x16(const uint32_t id, const double value);
can_msgs::msg::Frame getMsg(const uint32_t id);
uint32_t getApi(const can_msgs::msg::Frame msg);
uint32_t getDeviceNumber(const can_msgs::msg::Frame msg);

/**
* Comparing the raw bytes of the 16x16 fixed-point numbers
Expand All @@ -505,15 +505,15 @@ 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];
Field vcomp_fields_[5];
Field pos_fields_[7];
Field ictrl_fields_[6];
Field status_fields_[15];
Field cfg_fields_[15];
Field voltage_fields_[4];
Field spd_fields_[7];
Field vcomp_fields_[5];
Field pos_fields_[7];
Field ictrl_fields_[6];
Field status_fields_[15];
Field cfg_fields_[15];

Field* voltageFieldForMessage(uint32_t api);
Field* spdFieldForMessage(uint32_t api);
Expand Down
52 changes: 26 additions & 26 deletions puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,33 +117,33 @@ class MultiPumaNode : public rclcpp::Node
void run();

private:
std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
std::vector<puma_motor_driver::Driver> drivers_;

bool active_;
double gear_ratio_;
int encoder_cpr_;
int freq_;
uint8_t status_count_;
uint8_t desired_mode_;
std::string canbus_dev_;
std::vector<std::string> joint_names_;
std::vector<int64_t> joint_can_ids_;
std::vector<int64_t> joint_directions_;

can_msgs::msg::Frame::SharedPtr recv_msg_;
clearpath_motor_msgs::msg::PumaMultiStatus status_msg_;
clearpath_motor_msgs::msg::PumaMultiFeedback feedback_msg_;

double gain_p_;
double gain_i_;
double gain_d_;

rclcpp::Node::SharedPtr node_handle_;
rclcpp::Publisher<clearpath_motor_msgs::msg::PumaMultiStatus>::SharedPtr status_pub_;
std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
std::vector<puma_motor_driver::Driver> drivers_;

bool active_;
double gear_ratio_;
int encoder_cpr_;
int freq_;
uint8_t status_count_;
uint8_t desired_mode_;
std::string canbus_dev_;
std::vector<std::string> joint_names_;
std::vector<int64_t> joint_can_ids_;
std::vector<int64_t> joint_directions_;

can_msgs::msg::Frame::SharedPtr recv_msg_;
clearpath_motor_msgs::msg::PumaMultiStatus status_msg_;
clearpath_motor_msgs::msg::PumaMultiFeedback feedback_msg_;

double gain_p_;
double gain_i_;
double gain_d_;

rclcpp::Node::SharedPtr node_handle_;
rclcpp::Publisher<clearpath_motor_msgs::msg::PumaMultiStatus>::SharedPtr status_pub_;
rclcpp::Publisher<clearpath_motor_msgs::msg::PumaMultiFeedback>::SharedPtr feedback_pub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr cmd_sub_;
rclcpp::TimerBase::SharedPtr run_timer_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr cmd_sub_;
rclcpp::TimerBase::SharedPtr run_timer_;
};

#endif // PUMA_MOTOR_DRIVER_PUMA_NODE_H
66 changes: 33 additions & 33 deletions puma_motor_driver/src/driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg)
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)
{
Expand Down Expand Up @@ -142,8 +142,8 @@ void Driver::sendId(const uint32_t id)
void Driver::sendUint8(const uint32_t id, const uint8_t value)
{
can_msgs::msg::Frame msg = getMsg(id);
msg.dlc = sizeof(uint8_t);
uint8_t data[8] = { 0 };
msg.dlc = sizeof(uint8_t);
uint8_t data[8] = { 0 };
std::memcpy(data, &value, sizeof(uint8_t));
std::copy(std::begin(data), std::end(data), std::begin(msg.data));

Expand All @@ -153,8 +153,8 @@ void Driver::sendUint8(const uint32_t id, const uint8_t value)
void Driver::sendUint16(const uint32_t id, const uint16_t value)
{
can_msgs::msg::Frame msg = getMsg(id);
msg.dlc = sizeof(uint16_t);
uint8_t data[8] = { 0 };
msg.dlc = sizeof(uint16_t);
uint8_t data[8] = { 0 };
std::memcpy(data, &value, sizeof(uint16_t));
std::copy(std::begin(data), std::end(data), std::begin(msg.data));

Expand All @@ -164,10 +164,10 @@ void Driver::sendUint16(const uint32_t id, const uint16_t value)
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<int16_t>(static_cast<float>(1 << 8) * value);
msg.dlc = sizeof(int16_t);
int16_t output_value = static_cast<int16_t>(static_cast<float>(1 << 8) * value);

uint8_t data[8] = { 0 };
uint8_t data[8] = { 0 };
std::memcpy(data, &output_value, sizeof(int16_t));
std::copy(std::begin(data), std::end(data), std::begin(msg.data));

Expand All @@ -177,10 +177,10 @@ void Driver::sendFixed8x8(const uint32_t id, const float value)
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<int32_t>(static_cast<double>((1 << 16) * value));
msg.dlc = sizeof(int32_t);
int32_t output_value = static_cast<int32_t>(static_cast<double>((1 << 16) * value));

uint8_t data[8] = { 0 };
uint8_t data[8] = { 0 };
std::memcpy(data, &output_value, sizeof(int32_t));
std::copy(std::begin(data), std::end(data), std::begin(msg.data));

Expand All @@ -190,10 +190,10 @@ void Driver::sendFixed16x16(const uint32_t id, const double value)
can_msgs::msg::Frame Driver::getMsg(const uint32_t id)
{
can_msgs::msg::Frame msg;
msg.id = id;
msg.dlc = 0;
msg.is_extended = true;
msg.header.stamp = nh_->get_clock()->now();
msg.id = id;
msg.dlc = 0;
msg.is_extended = true;
msg.header.stamp = nh_->get_clock()->now();
msg.header.frame_id = "base_link";
return msg;
}
Expand Down Expand Up @@ -437,7 +437,7 @@ void Driver::verifyParams()
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;
state_ = ConfigurationState::Configured;
}
}

Expand Down Expand Up @@ -663,13 +663,13 @@ void Driver::requestFeedbackSetpoint()
void Driver::resetConfiguration()
{
configured_ = false;
state_ = ConfigurationState::Initializing;
state_ = ConfigurationState::Initializing;
}

void Driver::updateGains()
{
configured_ = false;
state_ = ConfigurationState::PGain;
state_ = ConfigurationState::PGain;
}

bool Driver::receivedDutyCycle()
Expand Down Expand Up @@ -786,77 +786,77 @@ bool Driver::receivedPositionSetpoint()

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;
}

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
}

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
}

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();
}
Expand Down Expand Up @@ -884,28 +884,28 @@ 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
}

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;
}

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
}
Expand Down
Loading

0 comments on commit 1e1583a

Please sign in to comment.