openarm/openarm_ros2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

fix: compatible with canfd (#7)

Browse Source 
## Summary

This PR updates the C++ hardware interface to support **CANFD** and confirms successful operation with **MoveIt** on the actual robot hardware.

---

##  Changes Made

- Migrated from classic CAN to **CANFD** in the motor control interface.
- Updated the following modules:
  - `openarm_hardware/include/openarm_hardware/canbus.hpp`
  - `openarm_hardware/src/canbus.cpp`
  - `openarm_hardware/src/motor_control.cpp`
  - `openarm_hardware/include/openarm_hardware/motor_control.hpp`
  - `openarm_hardware/src/openarm_hardware.cpp`
- Improved CAN socket handling and data frame structure.
- Refactored internal motor communication logic for robustness.

---

##  Verification

-  Confirmed joint control and motion execution with real hardware using **MoveIt**.
-  Controllers were properly loaded and executed planned trajectories without error.
-  CANFD communication is now functional and stable during runtime.
This commit is contained in:
toki 2025-05-19 11:32:09 +09:00 committed by GitHub
parent de9eb17e86
commit d6cc80c17e
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
11 changed files with 687 additions and 228 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
openarm_hardware/include/openarm_hardware/.openarm_hardware.hpp.swp Normal file
View File

Binary file not shown.

24
openarm_hardware/include/openarm_hardware/canbus.hpp
View File

@ -1,6 +1,6 @@
#ifndef CANBUS_H
#define CANBUS_H
#pragma once
#include <cstdint>
#include <iostream>
#include <cstring>
#include <unistd.h>
@ -10,17 +10,29 @@
#include <linux/can/raw.h>
#include <array>
#include <fcntl.h>
#include <string>
enum CANMode {
CAN_MODE_CLASSIC = 0,
CAN_MODE_FD = 1
};
class CANBus {
public:
explicit CANBus(const std::string& interface);
explicit CANBus(const std::string& interface, int mode);
~CANBus();
int whichCAN();
bool send(uint16_t motor_id, const std::array<uint8_t, 8>& data);
struct can_frame recv();
std::array<uint8_t, 64> recv(uint16_t& out_id, uint8_t& out_len);
private:
bool sendClassic(uint16_t motor_id, const std::array<uint8_t, 8>& data);
bool sendFD(uint16_t motor_id, const std::array<uint8_t, 8>& data);
struct can_frame recvClassic();
struct canfd_frame recvFD();
int sock_;
int mode_;
};
#endif // CANBUS_H

78
openarm_hardware/include/openarm_hardware/motor.hpp
View File

@ -1,5 +1,4 @@
#ifndef MOTOR_H
#define MOTOR_H
#pragma once
#include <cstdint>
#include <map>
@ -27,12 +26,51 @@ enum class DM_Motor_Type : uint8_t {
};
enum class DM_variable : uint8_t {
UV_Value = 0, KT_Value, OT_Value, OC_Value, ACC, DEC, MAX_SPD, MST_ID,
ESC_ID, TIMEOUT, CTRL_MODE, Damp, Inertia, hw_ver, sw_ver, SN, NPP, Rs,
LS, Flux, Gr, PMAX, VMAX, TMAX, I_BW, KP_ASR, KI_ASR, KP_APR, KI_APR,
OV_Value, GREF, Deta, V_BW, IQ_c1, VL_c1, can_br, sub_ver,
u_off = 50, v_off, k1, k2, m_off, dir,
p_m = 80, xout,
UV_Value = 0,
KT_Value,
OT_Value,
OC_Value,
ACC,
DEC,
MAX_SPD,
MST_ID,
ESC_ID,
TIMEOUT,
CTRL_MODE,
Damp,
Inertia,
hw_ver,
sw_ver,
SN,
NPP,
Rs,
LS,
Flux,
Gr,
PMAX,
VMAX,
TMAX,
I_BW,
KP_ASR,
KI_ASR,
KP_APR,
KI_APR,
OV_Value,
GREF,
Deta,
V_BW,
IQ_c1,
VL_c1,
can_br,
sub_ver,
u_off = 50,
v_off,
k1,
k2,
m_off,
dir,
p_m = 80,
xout,
COUNT
};
@ -54,7 +92,7 @@ public:
double getVelocity() const;
double getTorque() const;
int getParam(int RID) const;
void setTempParam(int RID, int val);
uint16_t SlaveID;
uint16_t MasterID;
bool isEnable;
@ -64,31 +102,45 @@ public:
int getStateTmos() const;
int getStateTrotor() const;
double getGoalPosition() const;
double getGoalVelocity() const;
double getGoalTau() const;
void setGoalPosition(double pos);
void setGoalVelocity(double vel);
void setGoalTau(double tau);
void setStateTmos(int tmos);
void setStateTrotor(int trotor);
private:
double Pd, Vd;
double goal_position, goal_tau;
double goal_position,goal_velocity, goal_tau;
double state_q, state_dq, state_tau;
int state_tmos, state_trotor;
std::map<int, int> temp_param_dict;
};
double LIMIT_MIN_MAX(double x, double min, double max);
uint16_t double_to_uint(double x, double x_min, double x_max, int bits);
double uint_to_double(uint16_t x, double min, double max, int bits);
std::array<uint8_t, 4> double_to_uint8s(double value);
std::array<uint8_t, 4> data_to_uint8s(uint32_t value);
std::array<uint8_t, 8> double_to_uint8s(double value);
std::array<uint8_t, 4> float_to_uint8s(float value);
float uint8s_to_float(const std::array<uint8_t, 4>& bytes);
std::array<uint8_t, 8> data_to_uint8s(uint32_t value);
uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4);
double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4);
bool is_in_ranges(int number);
void print_hex(const std::vector<uint8_t>& data);
template <typename T>
T get_enum_by_index(int index);
#endif // MOTOR_H

75
openarm_hardware/include/openarm_hardware/motor_control.hpp
View File

@ -1,5 +1,4 @@
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H
#pragma once
#include <iostream>
#include <vector>
@ -11,37 +10,61 @@
#include "canbus.hpp"
#include <atomic>
#include <functional>
#include <thread>
#include <chrono>
class MotorControl {
public:
explicit MotorControl(CANBus& canbus);
public:
explicit MotorControl(CANBus& canbus);
bool addMotor(Motor& motor);
void enable(Motor& motor);
void disable(Motor& motor);
void set_zero_position(Motor& motor);
void controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau);
void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau);
void sendData(uint16_t motor_id, const std::array<uint8_t,8>& data);
void recv();
void controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau);
void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau);
void control_delay(Motor& motor, double kp, double kd, double q, double dq, double tau, double delay);
void controlPosVel(Motor& motor,double q, double dq);
void controlPosVel2(Motor& motor,double q, double dq);
void controlVel(Motor& motor, double dq);
void controlVel2(Motor& motor, double dq);
void controlPosForce(Motor& motor, double q, double vel, double tau);
void controlPosForce2(Motor& motor, double q, double vel, double tau);
void enable(Motor& motor);
void disable(Motor& motor);
void set_zero_position(Motor& motor);
void recv();
void sendData(uint16_t motor_id, const std::array<uint8_t,8>& data);
bool addMotor(Motor& motor);
void recv_set_param_data();
bool switchControlMode(Motor& motor, Control_Type controlMode);
void save_motor_param(Motor &motor);
void change_limit_param();
//void change_limit_param(DM_Motor_Type motor_type, double PMAX, double VMAX, double TMAX);
private:
CANBus& canbus_;
void recv_set_param_data();
std::map<uint16_t, Motor*> motors_map;
private:
CANBus& canbus_;
static constexpr double Limit_Param[12][3] = {
{12.5, 30, 10}, {12.5, 50, 10}, {12.5, 8, 28}, {12.5, 10, 28},
{12.5, 45, 20}, {12.5, 45, 40}, {12.5, 45, 54}, {12.5, 25, 200},
{12.5, 20, 200}, {12.5, 280, 1}, {12.5, 45, 10}, {12.5, 45, 10}
};
std::map<uint16_t, Motor*> motors_map_;
static constexpr double Limit_Param[12][3] = {
{12.5, 30, 10}, // DM4310
{12.5, 50, 10}, // DM4310_48V
{12.5, 8, 28}, // DM4340
{12.5, 10, 28}, // DM4340_48V
{12.5, 45, 20}, // DM6006
{12.5, 45, 40}, // DM8006
{12.5, 45, 54}, // DM8009
{12.5, 25, 200}, // DM10010L
{12.5, 20, 200}, // DM10010
{12.5, 280, 1}, // DMH3510
{12.5, 45, 10}, // DMH6215
{12.5, 45, 10}, // DMG6220
};
void processPacket(const can_frame& frame);
void controlCmd(Motor &motor, uint8_t cmd );
void readRIDParam(Motor& motor, DM_variable RID);
void writeMotorParam(Motor& motor, DM_variable RID, double value);
void processPacket(const can_frame& frame);
void processPacketFD(const canfd_frame& frame);
void controlCmd(Motor &motor, uint8_t cmd );
void readRIDParam(Motor& motor, DM_variable RID);
void writeMotorParam(Motor& motor, DM_variable RID, double value);
};
#endif // MOTOR_CONTROL_H

5
openarm_hardware/include/openarm_hardware/openarm_hardware.hpp
View File

@ -13,8 +13,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_
#define OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_
#pragma once
#include <string>
#include <vector>
@ -32,6 +31,7 @@
#include "motor.hpp"
#include "motor_control.hpp"
namespace openarm_hardware
{
@ -106,4 +106,3 @@ private:
} // namespace openarm_hardware
#endif // OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_

5
openarm_hardware/include/openarm_hardware/visibility_control.h
View File

@ -13,9 +13,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef OPENARM_HARDWARE__VISIBILITY_CONTROL_H_
#define OPENARM_HARDWARE__VISIBILITY_CONTROL_H_
#pragma once
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
@ -47,4 +45,3 @@
#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE
#endif
#endif // OPENARM_HARDWARE__VISIBILITY_CONTROL_H_

BIN
openarm_hardware/src/.openarm_hardware.cpp.swp Normal file
View File

Binary file not shown.

81
openarm_hardware/src/canbus.cpp
View File

@ -1,7 +1,7 @@
#include "openarm_hardware/canbus.hpp"
#include <poll.h>
CANBus::CANBus(const std::string& interface) {
CANBus::CANBus(const std::string& interface, int mode)
: mode_(mode) {
struct ifreq ifr;
struct sockaddr_can addr;
@ -21,6 +21,14 @@ CANBus::CANBus(const std::string& interface) {
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (mode_ == CAN_MODE_FD) {
int enable_canfd = 1;
if (setsockopt(sock_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd)) < 0) {
perror("CAN FD setsockopt failed");
exit(EXIT_FAILURE);
}
}
if (bind(sock_, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
perror("Error in CAN socket bind");
exit(EXIT_FAILURE);
@ -31,7 +39,34 @@ CANBus::~CANBus() {
close(sock_);
}
int CANBus::whichCAN(){
return mode_;
}
bool CANBus::send(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
if (mode_ == CAN_MODE_FD)
return sendFD(motor_id, data);
else
return sendClassic(motor_id, data);
}
std::array<uint8_t, 64> CANBus::recv(uint16_t& out_id, uint8_t& out_len) {
std::array<uint8_t, 64> buffer = {};
if (mode_ == CAN_MODE_FD) {
auto frame = recvFD();
out_id = frame.can_id;
out_len = frame.len;
std::copy(frame.data, frame.data + frame.len, buffer.begin());
} else {
auto frame = recvClassic();
out_id = frame.can_id;
out_len = frame.can_dlc;
std::copy(frame.data, frame.data + frame.can_dlc, buffer.begin());
}
return buffer;
}
bool CANBus::sendClassic(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
struct can_frame frame;
std::memset(&frame, 0, sizeof(frame));
@ -42,27 +77,45 @@ bool CANBus::send(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) {
perror("Error sending CAN frame");
return false;
} else {
// std::cout << "Sent CAN frame to motor ID " << motor_id << std::endl;
return true;
}
return true;
}
struct can_frame CANBus::recv() {
bool CANBus::sendFD(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
struct canfd_frame frame;
std::memset(&frame, 0, sizeof(frame));
frame.can_id = motor_id;
frame.len = data.size();
frame.flags = CANFD_BRS;
std::copy(data.begin(), data.end(), frame.data);
if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) {
perror("Error sending CAN FD frame");
return false;
}
return true;
}
struct can_frame CANBus::recvClassic() {
struct can_frame frame;
std::memset(&frame, 0, sizeof(frame));
// change socket into non blocking mode
// int flags = fcntl(sock_, F_GETFL, 0);
// fcntl(sock_, F_SETFL, flags | O_NONBLOCK);
int nbytes = read(sock_, &frame, sizeof(struct can_frame));
if (nbytes < 0) {
perror("CAN read error");
} else {
// std::cout << "Received CAN frame from motor ID " << frame.can_id << std::endl;
}
return frame;
}
struct canfd_frame CANBus::recvFD() {
struct canfd_frame frame;
std::memset(&frame, 0, sizeof(frame));
int nbytes = read(sock_, &frame, sizeof(struct canfd_frame));
if (nbytes < 0) {
perror("CAN FD read error");
}
return frame;
}

141
openarm_hardware/src/motor.cpp
View File

@ -1,19 +1,22 @@
#ifndef OPENARM_MOTOR_H_
#define OPENARM_MOTOR_H_
#include "openarm_hardware/motor.hpp"
Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID)
: MotorType(motorType), SlaveID(slaveID), MasterID(masterID),
Pd(0.0), Vd(0.0), goal_position(0.0), goal_tau(0.0),
state_q(0.0), state_dq(0.0), state_tau(0.0),
state_tmos(0), state_trotor(0),
isEnable(false), NowControlMode(Control_Type::MIT) {}
: MotorType(motorType), SlaveID(slaveID), MasterID(masterID),
Pd(0.0), Vd(0.0), goal_position(0.0), goal_tau(0.0),
state_q(0.0), state_dq(0.0), state_tau(0.0),
state_tmos(0), state_trotor(0),
isEnable(false), NowControlMode(Control_Type::MIT) {}
void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) {
state_q = q;
state_dq = dq;
state_tau = tau;
state_tmos = tmos;
state_trotor = trotor;
}
void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) {
state_q = q;
state_dq = dq;
state_tau = tau;
state_tmos = tmos;
state_trotor = trotor;
}
double Motor::getPosition() const { return state_q; }
double Motor::getVelocity() const { return state_dq; }
@ -21,100 +24,128 @@ double Motor::getTorque() const { return state_tau; }
double Motor::getGoalPosition() const {
return goal_position;
return goal_position;
}
void Motor::setGoalPosition(double pos) {
goal_position = pos;
goal_position = pos;
}
double Motor::getGoalVelocity() const {
return goal_velocity;
}
void Motor::setGoalVelocity(double velocity) {
goal_velocity = velocity;
}
double Motor::getGoalTau() const {
return goal_tau;
return goal_tau;
}
void Motor::setGoalTau(double tau) {
goal_tau = tau;
goal_tau = tau;
}
int Motor::getStateTmos() const {
return state_tmos;
return state_tmos;
}
void Motor::setStateTmos(int tmos) {
state_tmos = tmos;
state_tmos = tmos;
}
int Motor::getStateTrotor() const {
return state_trotor;
return state_trotor;
}
void Motor::setStateTrotor(int trotor) {
state_trotor = trotor;
state_trotor = trotor;
}
int Motor::getParam(int RID) const {
auto it = temp_param_dict.find(RID);
return (it != temp_param_dict.end()) ? it->second : -1;
auto it = temp_param_dict.find(RID);
return (it != temp_param_dict.end()) ? it->second : -1;
}
void Motor::setTempParam(int RID, int value) {
temp_param_dict[RID] = value;
}
double LIMIT_MIN_MAX(double x, double min, double max) {
return std::max(min, std::min(x, max));
return std::max(min, std::min(x, max));
}
uint16_t double_to_uint(double x, double x_min, double x_max, int bits) {
x = LIMIT_MIN_MAX(x, x_min, x_max);
double span = x_max - x_min;
double data_norm = (x - x_min) / span;
return static_cast<uint16_t>(data_norm * ((1 << bits) - 1));
x = LIMIT_MIN_MAX(x, x_min, x_max);
double span = x_max - x_min;
double data_norm = (x - x_min) / span;
return static_cast<uint16_t>(data_norm * ((1 << bits) - 1));
}
double uint_to_double(uint16_t x, double min, double max, int bits) {
double span = max - min;
double data_norm = static_cast<double>(x) / ((1 << bits) - 1);
return data_norm * span + min;
double span = max - min;
double data_norm = static_cast<double>(x) / ((1 << bits) - 1);
return data_norm * span + min;
}
std::array<uint8_t, 4> double_to_uint8s(double value) {
std::array<uint8_t, 4> bytes;
std::memcpy(bytes.data(), &value, sizeof(double));
return bytes;
std::array<uint8_t, 8> double_to_uint8s(double value) {
std::array<uint8_t, 8> bytes;
std::memcpy(bytes.data(), &value, sizeof(double));
return bytes;
}
std::array<uint8_t, 4> data_to_uint8s(uint32_t value) {
std::array<uint8_t, 4> bytes;
std::memcpy(bytes.data(), &value, sizeof(uint32_t));
return bytes;
std::array<uint8_t, 4> float_to_uint8s(float value) {
std::array<uint8_t, 4> bytes{};
std::memcpy(bytes.data(), &value, sizeof(float));
return bytes;
}
float uint8s_to_float(const std::array<uint8_t, 4>& bytes) {
float value;
std::memcpy(&value, bytes.data(), sizeof(float));
return value;
}
std::array<uint8_t, 8> data_to_uint8s(uint32_t value) {
std::array<uint8_t, 8> bytes;
std::memcpy(bytes.data(), &value, sizeof(uint32_t));
return bytes;
}
uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) {
uint32_t value;
uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
std::memcpy(&value, bytes, sizeof(uint32_t));
return value;
uint32_t value;
uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
std::memcpy(&value, bytes, sizeof(uint32_t));
return value;
}
double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) {
double value;
uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
std::memcpy(&value, bytes, sizeof(double));
return value;
double value;
uint8_t bytes[4] = {byte1, byte2, byte3, byte4};
std::memcpy(&value, bytes, sizeof(double));
return value;
}
bool is_in_ranges(int number) {
return (7 <= number && number <= 10) || (13 <= number && number <= 16) || (35 <= number && number <= 36);
return (7 <= number && number <= 10) || (13 <= number && number <= 16) || (35 <= number && number <= 36);
}
void print_hex(const std::vector<uint8_t>& data) {
for (auto byte : data) {
std::cout << std::hex << std::uppercase << (int)byte << " ";
}
std::cout << std::dec << std::endl;
for (auto byte : data) {
std::cout << std::hex << std::uppercase << (int)byte << " ";
}
std::cout << std::dec << std::endl;
}
template <typename T>
T get_enum_by_index(int index) {
if (index >= 0 && index < static_cast<int>(T::COUNT)) {
return static_cast<T>(index);
}
return static_cast<T>(-1);
if (index >= 0 && index < static_cast<int>(T::COUNT)) {
return static_cast<T>(index);
}
return static_cast<T>(-1);
}
#endif // OPENARM_MOTOR_H_

502
openarm_hardware/src/motor_control.cpp
View File

@ -3,161 +3,453 @@
MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {}
bool MotorControl::addMotor(Motor& motor) {
motors_map[motor.SlaveID] = &motor;
if (motor.MasterID != 0) {
motors_map[motor.MasterID] = &motor;
}
return true;
motors_map_[motor.SlaveID] = &motor;
if (motor.MasterID != 0) {
motors_map_[motor.MasterID] = &motor;
}
return true;
}
void MotorControl::enable(Motor& motor) {
controlCmd(motor, 0xFC);
sleep(0.3);
controlCmd(motor, 0xFC);
sleep(0.3);
}
void MotorControl::disable(Motor& motor) {
controlCmd(motor, 0xFD);
sleep(0.3);
controlCmd(motor, 0xFD);
sleep(0.3);
}
void MotorControl::set_zero_position(Motor& motor){
controlCmd(motor, 0xFE);
sleep(0.3);
recv();
controlCmd(motor, 0xFE);
sleep(0.3);
recv();
}
void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau) {
controlMIT2(motor, kp, kd, q, dq, tau);
recv();
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t kp_uint = double_to_uint(kp, 0, 500, 12);
uint16_t kd_uint = double_to_uint(kd, 0, 5, 12);
int motor_index = static_cast<int>(motor.MotorType);
double Q_MAX = Limit_Param[motor_index][0];
double DQ_MAX = Limit_Param[motor_index][1];
double TAU_MAX = Limit_Param[motor_index][2];
uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16);
uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12);
uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12);
std::array<uint8_t, 8> data = {
static_cast<uint8_t>((q_uint >> 8) & 0xFF),
static_cast<uint8_t>(q_uint & 0xFF),
static_cast<uint8_t>(dq_uint >> 4),
static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)),
static_cast<uint8_t>(kp_uint & 0xFF),
static_cast<uint8_t>(kd_uint >> 4),
static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)),
static_cast<uint8_t>(tau_uint & 0xFF)
};
sendData(motor.SlaveID, data);
recv();
}
void MotorControl::controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau) {
if (motors_map.find(motor.SlaveID) == motors_map.end()) {
std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t kp_uint = double_to_uint(kp, 0, 500, 12);
uint16_t kd_uint = double_to_uint(kd, 0, 5, 12);
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl;
return;
}
int motor_index = static_cast<int>(motor.MotorType);
double Q_MAX = Limit_Param[motor_index][0];
double DQ_MAX = Limit_Param[motor_index][1];
double TAU_MAX = Limit_Param[motor_index][2];
uint16_t kp_uint = double_to_uint(kp, 0, 500, 12);
uint16_t kd_uint = double_to_uint(kd, 0, 5, 12);
uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16);
uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12);
uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12);
int motor_index = static_cast<int>(motor.MotorType);
double Q_MAX = Limit_Param[motor_index][0];
double DQ_MAX = Limit_Param[motor_index][1];
double TAU_MAX = Limit_Param[motor_index][2];
std::array<uint8_t, 8> data = {
static_cast<uint8_t>((q_uint >> 8) & 0xFF),
static_cast<uint8_t>(q_uint & 0xFF),
static_cast<uint8_t>(dq_uint >> 4),
static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)),
static_cast<uint8_t>(kp_uint & 0xFF),
static_cast<uint8_t>(kd_uint >> 4),
static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)),
static_cast<uint8_t>(tau_uint & 0xFF)
};
uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16);
uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12);
uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12);
sendData(motor.SlaveID, data);
std::array<uint8_t, 8> data = {
static_cast<uint8_t>((q_uint >> 8) & 0xFF),
static_cast<uint8_t>(q_uint & 0xFF),
static_cast<uint8_t>(dq_uint >> 4),
static_cast<uint8_t>(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)),
static_cast<uint8_t>(kp_uint & 0xFF),
static_cast<uint8_t>(kd_uint >> 4),
static_cast<uint8_t>(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)),
static_cast<uint8_t>(tau_uint & 0xFF)
};
sendData(motor.SlaveID, data);
}
void MotorControl::sendData(uint16_t motor_id, const std::array<uint8_t, 8>& data) {
canbus_.send(motor_id, data);
canbus_.send(motor_id, data);
}
void MotorControl::recv() {
can_frame frame = canbus_.recv();
processPacket(frame);
uint16_t id;
uint8_t len;
std::array<uint8_t, 64> data = canbus_.recv(id, len);
if (canbus_.whichCAN() == CAN_MODE_CLASSIC) {
can_frame frame;
std::memset(&frame, 0, sizeof(frame));
frame.can_id = id;
frame.can_dlc = len;
std::memcpy(frame.data, data.data(), len);
processPacket(frame);
}
else if (canbus_.whichCAN() == CAN_MODE_FD) {
canfd_frame fd_frame;
std::memset(&fd_frame, 0, sizeof(fd_frame));
fd_frame.can_id = id;
fd_frame.len = len;
std::memcpy(fd_frame.data, data.data(), len);
processPacketFD(fd_frame);
}
}
void MotorControl::control_delay(Motor& motor, double kp, double kd, double q, double dq, double tau, double delay){
controlMIT(motor, kp, kd, q, dq, tau);
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(delay)));
}
void MotorControl::controlPosVel(Motor& motor, double pos, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosVel ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t motor_id = 0x100 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0};
auto vel_buf = float_to_uint8s(static_cast<float>(vel));
auto pos_buf = float_to_uint8s(static_cast<float>(pos));
for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i];
data_buf[i + 4] = vel_buf[i];
}
sendData(motor_id, data_buf);
recv();
}
void MotorControl::controlPosVel2(Motor& motor, double pos, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosVel2 ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t motor_id = 0x100 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0};
auto pos_buf = float_to_uint8s(static_cast<float>(pos));
auto vel_buf = float_to_uint8s(static_cast<float>(vel));
for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i];
data_buf[i + 4] = vel_buf[i];
}
sendData(motor_id, data_buf);
}
void MotorControl::controlVel(Motor& motor, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlVel ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t motor_id = 0x200 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0};
auto vel_buf = float_to_uint8s(static_cast<float>(vel));
for (int i = 0; i < 4; ++i) {
data_buf[i] = vel_buf[i];
}
sendData(motor_id, data_buf);
recv();
}
void MotorControl::controlVel2(Motor& motor, double vel) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlVel2 ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t motor_id = 0x200 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0};
auto vel_buf = float_to_uint8s(static_cast<float>(vel));
for (int i = 0; i < 4; ++i) {
data_buf[i] = vel_buf[i];
}
sendData(motor_id, data_buf);
}
void MotorControl::controlPosForce(Motor& motor, double pos, double vel, double tau) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t motor_id = 0x300 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0};
auto pos_buf = float_to_uint8s(static_cast<float>(pos));
auto vel_buf = float_to_uint8s(static_cast<float>(vel));
auto tau_buf = float_to_uint8s(static_cast<float>(tau));
for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i];
}
data_buf[4] = vel_buf[0];
data_buf[5] = vel_buf[1];
data_buf[6] = tau_buf[0];
data_buf[7] = tau_buf[1];
sendData(motor_id, data_buf);
recv();
}
void MotorControl::controlPosForce2(Motor& motor, double pos, double vel, double tau) {
if (motors_map_.find(motor.SlaveID) == motors_map_.end()) {
std::cerr << "controlPosForce ERROR: Motor ID not found" << std::endl;
return;
}
uint16_t motor_id = 0x300 + motor.SlaveID;
std::array<uint8_t, 8> data_buf = {0};
auto pos_buf = float_to_uint8s(static_cast<float>(pos));
auto vel_buf = float_to_uint8s(static_cast<float>(vel));
auto tau_buf = float_to_uint8s(static_cast<float>(tau));
for (int i = 0; i < 4; ++i) {
data_buf[i] = pos_buf[i];
}
data_buf[4] = vel_buf[0];
data_buf[5] = vel_buf[1];
data_buf[6] = tau_buf[0];
data_buf[7] = tau_buf[1];
sendData(motor_id, data_buf);
}
bool MotorControl::switchControlMode(Motor& motor, Control_Type control_mode) {
const int max_retries = 20;
const double retry_interval = 0.1;
DM_variable RID = DM_variable::CTRL_MODE;
writeMotorParam(motor, RID, static_cast<int>(control_mode));
for (int i = 0; i < max_retries; ++i) {
usleep(static_cast<useconds_t>(retry_interval * 1e6));
recv_set_param_data();
if (motor.getParam(static_cast<int>(RID)) == static_cast<int>(control_mode)) {
return true;
}
}
return false;
}
void MotorControl::save_motor_param(Motor& motor) {
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0xAA, 0x00, 0x00, 0x00, 0x00, 0x00
};
disable(motor);
canbus_.send(0x7FF, data);
usleep(1000);
}
//void MotorControl::change_limit_param(DM_Motor_Type motor_type, double PMAX, double VMAX, double TMAX) {
//int index = static_cast<int>(motor_type);
//Limit_Param[index][0] = PMAX;
//Limit_Param[index][1] = VMAX;
//Limit_Param[index][2] = TMAX;
//}
void MotorControl::recv_set_param_data() {
uint16_t id;
uint8_t len;
std::array<uint8_t, 64> data = canbus_.recv(id, len);
uint8_t cmd = 0x11;
if (len >= 8) {
std::cout << "CANID: 0x" << std::hex << id << ", CMD: 0x" << static_cast<int>(cmd) << std::dec << std::endl;
for (int i = 0; i < 8; ++i) {
std::cout << "0x" << std::hex << static_cast<int>(data[i]) << " ";
}
std::cout << std::dec << std::endl;
}
}
void MotorControl::processPacket(const can_frame& frame) {
uint16_t motorID = frame.data[0];
uint8_t cmd = 0x11;
uint16_t motorID = frame.data[0];
uint8_t cmd = 0x11; // someday fix
if (cmd == 0x11) {
if (motorID != 0x00) {
auto it = motors_map.find(motorID);
if (it != motors_map.end() && it->second) {
Motor* motor = it->second;
if (cmd == 0x11) {
if (motorID != 0x00) {
auto it = motors_map_.find(motorID);
if (it != motors_map_.end() && it->second) {
Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
} else {
uint16_t MasterID = frame.data[0] & 0x0F;
auto it = motors_map.find(MasterID);
if (it != motors_map.end() && it->second) {
Motor* motor = it->second;
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
} else {
uint16_t MasterID = frame.data[0] & 0x0F;
auto it = motors_map_.find(MasterID);
if (it != motors_map_.end() && it->second) {
Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
}
}
}
}
void MotorControl::processPacketFD(const canfd_frame& frame) {
uint16_t motorID = frame.data[0];
uint8_t cmd = 0x11; // someday fix
if (cmd == 0x11) {
if (motorID != 0x00) {
auto it = motors_map_.find(motorID);
if (it != motors_map_.end() && it->second) {
Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
} else {
uint16_t MasterID = frame.data[0] & 0x0F;
auto it = motors_map_.find(MasterID);
if (it != motors_map_.end() && it->second) {
Motor* motor = it->second;
uint16_t q_uint = (frame.data[1] << 8) | frame.data[2];
uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4);
uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5];
int t_mos = frame.data[6];
int t_rotor = frame.data[7];
double Q_MAX = Limit_Param[static_cast<int>(motor->MotorType)][0];
double DQ_MAX = Limit_Param[static_cast<int>(motor->MotorType)][1];
double TAU_MAX = Limit_Param[static_cast<int>(motor->MotorType)][2];
double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16);
double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12);
double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12);
motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor);
}
}
}
}
void MotorControl::controlCmd(Motor& motor, uint8_t cmd) {
std::array<uint8_t, 8> data_buf = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd};
sendData(motor.SlaveID, data_buf);
std::array<uint8_t, 8> data_buf = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd};
sendData(motor.SlaveID, data_buf);
}
void MotorControl::readRIDParam(Motor& motor, DM_variable RID) {
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x33,
static_cast<uint8_t>(RID),
0x00, 0x00, 0x00, 0x00
};
canbus_.send(0x7FF, data);
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x33,
static_cast<uint8_t>(RID),
0x00, 0x00, 0x00, 0x00
};
canbus_.send(0x7FF, data);
}
void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, double value) {
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x55,
static_cast<uint8_t>(RID)
};
std::array<uint8_t, 8> data = {
static_cast<uint8_t>(motor.SlaveID & 0xFF),
static_cast<uint8_t>((motor.SlaveID >> 8) & 0xFF),
0x55,
static_cast<uint8_t>(RID)
};
if (is_in_ranges(static_cast<int>(RID))) {
auto intData = data_to_uint8s(static_cast<uint32_t>(value));
std::copy(intData.begin(), intData.end(), data.begin() + 4);
} else {
auto doubleData = double_to_uint8s(value);
std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4);
}
if (is_in_ranges(static_cast<int>(RID))) {
auto intData = data_to_uint8s(static_cast<uint32_t>(value));
std::copy(intData.begin(), intData.end(), data.begin() + 4);
} else {
auto doubleData = double_to_uint8s(value);
std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4);
}
canbus_.send(0x7FF, data);
canbus_.send(0x7FF, data);
}

4
openarm_hardware/src/openarm_hardware.cpp
View File

@ -57,8 +57,8 @@ hardware_interface::CallbackReturn OpenArmHW::on_init(
disable_torque_ = it->second == "true";
}
canbus_ = std::make_unique<CANBus>(info.hardware_parameters.at("can_device"));
//temp CANFD
canbus_ = std::make_unique<CANBus>(info.hardware_parameters.at("can_device"),CAN_MODE_FD);
motor_control_ = std::make_unique<MotorControl>(*canbus_);
if(USING_GRIPPER){