From 2f76d3c284742bf3a56cb1bffeb02242eb4d773b Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 7 Oct 2018 13:29:18 -0400 Subject: [PATCH] Implement can_simple --- Firmware/communication/can_simple.cpp | 235 ++++++++++++++++++-------- Firmware/communication/can_simple.hpp | 55 +++--- 2 files changed, 192 insertions(+), 98 deletions(-) diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index c7b42ca0..8b2567c7 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -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(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(&(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(&(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); } \ No newline at end of file diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index a20c1703..f1b63998 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -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 //