Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-23 09:03:40 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Implement can_simple

Browse Source
This commit is contained in:
Unknown
2018-10-07 13:29:18 -04:00
parent 8373771457
commit 2f76d3c284
2 changed files with 192 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/communication/can_simple.cpp
+167 -68
View File
@@ -15,8 +15,10 @@ void CANSimple::handle_can_message(CAN_message_t& msg) {
// Frame
// nodeID | CMD
// 4 bits | 7 bits
auto nodeID = get_node_id(msg.id);
// 6 bits | 5 bits
uint32_t nodeID = get_node_id(msg.id);
uint32_t cmd = get_cmd_id(msg.id);
Axis* axis = nullptr;
for (uint8_t i = 0; i < AXIS_COUNT; i++) {
@@ -24,97 +26,177 @@ void CANSimple::handle_can_message(CAN_message_t& msg) {
axis = axes[i];
}
}
if (axis != nullptr) {
switch (get_cmd_id(msg.id)) {
case MSG_MOVE_TO_POS:
move_to_pos_callback(axis, msg);
break;
case MSG_SET_POS_SETPOINT:
set_pos_setpoint_callback(axis, msg);
break;
case MSG_SET_VEL_SETPOINT:
set_vel_setpoint_callback(axis, msg);
break;
case MSG_SET_CUR_SETPOINT:
set_current_setpoint_callback(axis, msg);
break;
}
switch (cmd) {
case MSG_CO_NMT_CTRL:
nmt_callback(axis, msg);
break;
case MSG_CO_SYNC_CTRL:
sync_callback(axis, msg);
break;
case MSG_CO_HEARTBEAT_CMD:
break;
case MSG_GET_ENCODER_ERROR:
get_encoder_error_callback(axis, msg);
break;
case MSG_GET_SENSORLESS_ERROR:
get_sensorless_error_callback(axis, msg);
break;
case MSG_SET_AXIS_NODE_ID:
set_axis_nodeid_callback(axis, msg);
break;
case MSG_SET_AXIS_REQUESTED_STATE:
set_axis_requested_state_callback(axis, msg);
break;
case MSG_SET_AXIS_STARTUP_CONFIG:
set_axis_startup_config_callback(axis, msg);
break;
case MSG_GET_ENCODER_ESTIMATES:
get_encoder_estimates_callback(axis, msg);
break;
case MSG_MOVE_TO_POS:
move_to_pos_callback(axis, msg);
break;
case MSG_SET_POS_SETPOINT:
set_pos_setpoint_callback(axis, msg);
break;
case MSG_SET_VEL_SETPOINT:
set_vel_setpoint_callback(axis, msg);
break;
case MSG_SET_CUR_SETPOINT:
set_current_setpoint_callback(axis, msg);
break;
case MSG_SET_VEL_LIMIT:
set_vel_limit_callback(axis, msg);
break;
case MSG_START_ANTICOGGING:
start_anticogging_callback(axis, msg);
break;
default:
break;
}
}
uint8_t CANSimple::get_node_id(uint32_t msgID){
return ((msgID >> NUM_NODE_ID_BITS) & 0x03F); // Upper 6 bits
void CANSimple::nmt_callback(Axis* axis, CAN_message_t& msg) {
// Not implemented
}
uint8_t CANSimple::get_cmd_id(uint32_t msgID){
return (msgID & 0x01F); // Bottom 5 bits
void CANSimple::sync_callback(Axis* axis, CAN_message_t& msg) {
// Not implemented
}
void CANSimple::estop_callback(){
for(Axis* axis : axes){
axis->error_ |= Axis::ERROR_ESTOP_REQUESTED;
}
void CANSimple::estop_callback(Axis* axis, CAN_message_t& msg) {
axis->error_ |= Axis::ERROR_ESTOP_REQUESTED;
}
void CANSimple::get_motor_error_callback(Axis* axis, CAN_message_t& msg) {
CAN_message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_MOTOR_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.len = 8;
txmsg.buf[0] = axis->motor_.error_;
txmsg.buf[1] = axis->motor_.error_ >> 8;
txmsg.buf[2] = axis->motor_.error_ >> 16;
txmsg.buf[3] = axis->motor_.error_ >> 24;
odCAN->write(txmsg);
}
void CANSimple::get_encoder_error_callback(Axis* axis, CAN_message_t& msg) {
CAN_message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.len = 8;
txmsg.buf[0] = axis->encoder_.error_;
txmsg.buf[1] = axis->encoder_.error_ >> 8;
txmsg.buf[2] = axis->encoder_.error_ >> 16;
txmsg.buf[3] = axis->encoder_.error_ >> 24;
odCAN->write(txmsg);
}
void CANSimple::get_sensorless_error_callback(Axis* axis, CAN_message_t& msg) {
CAN_message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_SENSORLESS_ERROR; // heartbeat ID
txmsg.isExt = false;
txmsg.len = 8;
txmsg.buf[0] = axis->sensorless_estimator_.error_;
txmsg.buf[1] = axis->sensorless_estimator_.error_ >> 8;
txmsg.buf[2] = axis->sensorless_estimator_.error_ >> 16;
txmsg.buf[3] = axis->sensorless_estimator_.error_ >> 24;
odCAN->write(txmsg);
}
void CANSimple::set_axis_nodeid_callback(Axis* axis, CAN_message_t& msg) {
axis->config_.can_node_id = msg.buf[0];
}
void CANSimple::set_axis_requested_state_callback(Axis* axis, CAN_message_t& msg) {
axis->requested_state_ = static_cast<Axis::State_t>(get_16bit_val(msg, 0));
}
void CANSimple::set_axis_startup_config_callback(Axis* axis, CAN_message_t& msg) {
// Not Implemented
}
void CANSimple::get_encoder_estimates_callback(Axis* axis, CAN_message_t& msg) {
CAN_message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_GET_ENCODER_ESTIMATES; // heartbeat ID
txmsg.isExt = false;
txmsg.len = 8;
uint32_t floatBytes = *(reinterpret_cast<int32_t*>(&(axis->encoder_.pos_estimate_)));
txmsg.buf[0] = floatBytes;
txmsg.buf[1] = floatBytes >> 8;
txmsg.buf[2] = floatBytes >> 16;
txmsg.buf[3] = floatBytes >> 24;
floatBytes = *(reinterpret_cast<int32_t*>(&(axis->encoder_.vel_estimate_)));
txmsg.buf[4] = floatBytes;
txmsg.buf[5] = floatBytes >> 8;
txmsg.buf[6] = floatBytes >> 16;
txmsg.buf[7] = floatBytes >> 24;
odCAN->write(txmsg);
}
void CANSimple::move_to_pos_callback(Axis* axis, CAN_message_t& msg) {
float pos = msg.buf[0];
pos += msg.buf[1] << 8;
pos += msg.buf[2] << 16;
pos += msg.buf[3] << 24;
axis->controller_.move_to_pos(pos);
axis->controller_.move_to_pos(get_32bit_val(msg, 0));
}
void CANSimple::set_pos_setpoint_callback(Axis* axis, CAN_message_t& msg) {
float pos = msg.buf[0];
pos += msg.buf[1] << 8;
pos += msg.buf[2] << 16;
pos += msg.buf[3] << 24;
float vel = msg.buf[4];
vel += msg.buf[5] << 8;
vel *= 0.1f; // Factor of 10
float current = msg.buf[6];
current += (msg.buf[7] << 8);
current *= 0.01f; // Factor of 100
axis->controller_.set_pos_setpoint(pos, vel, current);
axis->controller_.set_pos_setpoint(get_32bit_val(msg, 0), get_16bit_val(msg, 4) * 0.1f, get_16bit_val(msg, 6) * 0.01f);
}
void CANSimple::set_vel_setpoint_callback(Axis* axis, CAN_message_t& msg) {
float vel = msg.buf[0];
vel += msg.buf[1] << 8;
vel += msg.buf[2] << 16;
vel += msg.buf[3] << 24;
vel *= 0.01f;
float current = msg.buf[4];
current += msg.buf[5] << 8;
current += msg.buf[6] << 16;
current += msg.buf[7] << 24;
current *= 0.01f;
axis->controller_.set_vel_setpoint(vel, current);
axis->controller_.set_vel_setpoint(get_32bit_val(msg, 0) * 0.01f, get_32bit_val(msg, 4) * 0.01f);
}
void CANSimple::set_current_setpoint_callback(Axis* axis, CAN_message_t& msg) {
float current = msg.buf[0];
current += msg.buf[1] << 8;
current += msg.buf[2] << 16;
current += msg.buf[3] << 24;
current *= 0.01f;
axis->controller_.set_current_setpoint(current);
axis->controller_.set_current_setpoint(get_32bit_val(msg, 0) * 0.01f);
}
void CANSimple::send_heartbeat(Axis* axis){
void CANSimple::set_vel_limit_callback(Axis* axis, CAN_message_t& msg) {
axis->controller_.config_.vel_limit = get_32bit_val(msg, 0);
}
void CANSimple::start_anticogging_callback(Axis* axis, CAN_message_t& msg) {
axis->controller_.start_anticogging_calibration();
}
void CANSimple::send_heartbeat(Axis* axis) {
CAN_message_t txmsg;
txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS;
txmsg.id += MSG_HEARTBEAT_CMD; // heartbeat ID
txmsg.id += MSG_CO_HEARTBEAT_CMD; // heartbeat ID
txmsg.isExt = false;
txmsg.len = 8;
// Axis errors in 1st 32-bit value
txmsg.buf[0] = axis->error_;
txmsg.buf[1] = axis->error_ >> 8;
@@ -127,4 +209,21 @@ void CANSimple::send_heartbeat(Axis* axis){
txmsg.buf[6] = axis->current_state_ >> 16;
txmsg.buf[7] = axis->current_state_ >> 24;
odCAN->write(txmsg);
}
uint8_t CANSimple::get_node_id(uint32_t msgID) {
return ((msgID >> NUM_CMD_ID_BITS) & 0x03F); // Upper 6 bits
}
uint8_t CANSimple::get_cmd_id(uint32_t msgID) {
return (msgID & 0x01F); // Bottom 5 bits
}
uint16_t CANSimple::get_16bit_val(CAN_message_t& msg, uint8_t start_byte) {
return msg.buf[start_byte] + (msg.buf[start_byte + 1] << 8);
}
uint32_t CANSimple::get_32bit_val(CAN_message_t& msg, uint8_t start_byte) {
return get_16bit_val(msg, 0) + (get_16bit_val(msg, 2) << 16);
// return msg.buf[start_byte] + (msg.buf[start_byte+1] << 8) + (msg.buf[start_byte+1] << 16) + (msg.buf[start_byte+1] << 24);
}
Firmware/communication/can_simple.hpp
+25 -30
View File
@@ -6,36 +6,18 @@
class CANSimple {
public:
enum {
MSG_NMT_CTRL = 0x000, // CANOpen NMT Message
MSG_SYNC_CTRL = 0x080, // CANOpen SYNC message
MSG_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat
MSG_EMERGENCY = 0x080, // CANOpen Emergency Message
MSG_GET_MOTOR_ERROR = 0x001, // Errors
MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC
MSG_CO_SYNC_CTRL = 0x080, // CANOpen SYNC message REC
MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND
MSG_CO_EMERGENCY = 0x080, // CANOpen Emergency Message SEND
MSG_GET_MOTOR_ERROR = 0x001, // Errors
MSG_GET_ENCODER_ERROR,
MSG_GET_CONTROLLER_ERROR,
MSG_GET_SENSORLESS_ERROR,
MSG_GET_VBUS_VOLTAGE, // ODrive-level properties
MSG_GET_SERIAL_NUMBER,
MSG_GET_HW_VERSION,
MSG_GET_FW_VERSION,
MSG_REBOOT_ODRIVE,
MSG_SAVE_CONFIG,
MSG_ERASE_CONFIG,
MSG_SET_AXIS_NODE_ID, // Axis properties
MSG_SET_AXIS_NODE_ID,
MSG_SET_AXIS_REQUESTED_STATE,
MSG_SET_STARTUP_CONFIG,
MSG_SET_MOTOR_PRECALIBRATED, // Motor properties
MSG_GET_MOTOR_CURRENT_LIM,
MSG_SET_MOTOR_CURRENT_LIM,
MSG_SET_MOTOR_POLE_PAIRS,
MSG_GET_ENCODER_CPR, // Encoder Properties
MSG_SET_ENCODER_CPR,
MSG_GET_ENCODER_INDEX_FOUND,
MSG_GET_ENCODER_POS_ESTIMATE,
MSG_GET_ENCODER_VEL_ESTIMATE,
MSG_SET_ENCODER_USE_INDEX,
MSG_SET_ENCODER_PRECALIBRATED,
MSG_MOVE_TO_POS, // Controller properties
MSG_SET_AXIS_STARTUP_CONFIG,
MSG_GET_ENCODER_ESTIMATES,
MSG_MOVE_TO_POS,
MSG_SET_POS_SETPOINT,
MSG_SET_VEL_SETPOINT,
MSG_SET_CUR_SETPOINT,
@@ -47,18 +29,31 @@ class CANSimple {
static void send_heartbeat(Axis* axis);
private:
static void estop_callback();
// Controller
static void nmt_callback(Axis* axis, CAN_message_t& msg);
static void sync_callback(Axis* axis, CAN_message_t& msg);
static void estop_callback(Axis* axis, CAN_message_t& msg);
static void get_motor_error_callback(Axis* axis, CAN_message_t& msg);
static void get_encoder_error_callback(Axis* axis, CAN_message_t& msg);
static void get_controller_error_callback(Axis* axis, CAN_message_t& msg);
static void get_sensorless_error_callback(Axis* axis, CAN_message_t& msg);
static void set_axis_nodeid_callback(Axis* axis, CAN_message_t& msg);
static void set_axis_requested_state_callback(Axis* axis, CAN_message_t& msg);
static void set_axis_startup_config_callback(Axis* axis, CAN_message_t& msg);
static void get_encoder_estimates_callback(Axis* axis, CAN_message_t& msg);
static void move_to_pos_callback(Axis* axis, CAN_message_t& msg);
static void set_pos_setpoint_callback(Axis* axis, CAN_message_t& msg);
static void set_vel_setpoint_callback(Axis* axis, CAN_message_t& msg);
static void set_current_setpoint_callback(Axis* axis, CAN_message_t& msg);
static void set_vel_limit_callback(Axis* axis, CAN_message_t& msg);
static void start_anticogging_callback(Axis* axis, CAN_message_t& msg);
// Utility functions
static uint8_t get_node_id(uint32_t msgID);
static uint8_t get_cmd_id(uint32_t msgID);
static uint16_t get_16bit_val(CAN_message_t& msg, uint8_t start_byte);
static uint32_t get_32bit_val(CAN_message_t& msg, uint8_t start_byte);
// This functional way of handling the messages is neat and is much cleaner from
// a data security point of view, but it will require some tweaking
//