From 1e40897c2add38bd92cfb089af229b6ba0185ee6 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 25 May 2019 14:52:01 +0200 Subject: [PATCH] Fix CANSimple to use input_pos, vel, current --- Firmware/communication/can_simple.cpp | 40 ++++++++++++++------------- Firmware/communication/can_simple.hpp | 20 +++++++------- docs/can-protocol.md | 17 ++++++------ 3 files changed, 40 insertions(+), 37 deletions(-) diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index 44f3844f..8efa2e13 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -76,17 +76,17 @@ void CANSimple::handle_can_message(can_Message_t& msg) { case MSG_GET_ENCODER_COUNT: get_encoder_count_callback(axis, msg); break; - case MSG_MOVE_TO_POS: - move_to_pos_callback(axis, msg); + case MSG_SET_INPUT_POS: + set_input_pos_callback(axis, msg); break; - case MSG_SET_POS_SETPOINT: - set_pos_setpoint_callback(axis, msg); + case MSG_SET_INPUT_VEL: + set_input_pos_callback(axis, msg); break; - case MSG_SET_VEL_SETPOINT: - set_vel_setpoint_callback(axis, msg); + case MSG_SET_INPUT_CURRENT: + set_input_pos_callback(axis, msg); break; - case MSG_SET_CUR_SETPOINT: - set_current_setpoint_callback(axis, msg); + case MSG_SET_CONTROLLER_MODES: + set_controller_modes_callback(axis, msg); break; case MSG_SET_VEL_LIMIT: set_vel_limit_callback(axis, msg); @@ -275,23 +275,25 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) { } } -void CANSimple::move_to_pos_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.move_to_pos(can_getSignal(msg, 0, 32, true, 1, 0)); +void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) { + axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true, 1, 0); + axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); + axis->controller_.input_current_ = can_getSignal(msg, 48, 16, true, 0.01f, 0); + axis->controller_.input_pos_updated(); } -void CANSimple::set_pos_setpoint_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.pos_setpoint_ = can_getSignal(msg, 0, 32, true, 1, 0); - axis->controller_.vel_setpoint_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); - axis->controller_.current_setpoint_ = can_getSignal(msg, 48, 16, true, 0.01f, 0); +void CANSimple::set_input_vel_callback(Axis* axis, can_Message_t& msg) { + axis->controller_.input_vel_ = can_getSignal(msg, 0, 32, true, 0.01f, 0.0f); + axis->controller_.input_current_ = can_getSignal(msg, 32, 16, true, 0.01f, 0.0f); } -void CANSimple::set_vel_setpoint_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.vel_setpoint_ = can_getSignal(msg, 0, 32, true, 0.01f, 0.0f); - axis->controller_.current_setpoint_ = can_getSignal(msg, 32, 16, true, 0.01f, 0.0f); +void CANSimple::set_input_current_callback(Axis* axis, can_Message_t& msg) { + axis->controller_.input_current_ = can_getSignal(msg, 0, 32, true, 0.01f, 0); } -void CANSimple::set_current_setpoint_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.current_setpoint_ = can_getSignal(msg, 0, 32, true, 0.01f, 0); +void CANSimple::set_controller_modes_callback(Axis* axis, can_Message_t& msg){ + axis->controller_.config_.control_mode = can_getSignal(msg, 0, 32, true, 1, 0); + axis->controller_.config_.input_mode = can_getSignal(msg, 32, 32, true, 1, 0); } void CANSimple::set_vel_limit_callback(Axis* axis, can_Message_t& msg) { diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index 75553b87..c2e16a61 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -7,8 +7,7 @@ class CANSimple { public: enum { MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC - MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND - MSG_ODRIVE_HEARTBEAT = 0x001, + MSG_ODRIVE_HEARTBEAT, MSG_ODRIVE_ESTOP, MSG_GET_MOTOR_ERROR, // Errors MSG_GET_ENCODER_ERROR, @@ -18,10 +17,10 @@ class CANSimple { MSG_SET_AXIS_STARTUP_CONFIG, MSG_GET_ENCODER_ESTIMATES, MSG_GET_ENCODER_COUNT, - MSG_MOVE_TO_POS, - MSG_SET_POS_SETPOINT, - MSG_SET_VEL_SETPOINT, - MSG_SET_CUR_SETPOINT, + MSG_SET_CONTROLLER_MODES, + MSG_SET_INPUT_POS, + MSG_SET_INPUT_VEL, + MSG_SET_INPUT_CURRENT, MSG_SET_VEL_LIMIT, MSG_START_ANTICOGGING, MSG_SET_TRAJ_VEL_LIMIT, @@ -31,6 +30,7 @@ class CANSimple { MSG_GET_SENSORLESS_ESTIMATES, MSG_RESET_ODRIVE, MSG_GET_VBUS_VOLTAGE, + MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND }; static void handle_can_message(can_Message_t& msg); @@ -48,10 +48,10 @@ class CANSimple { 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 get_encoder_count_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_input_pos_callback(Axis* axis, can_Message_t& msg); + static void set_input_vel_callback(Axis* axis, can_Message_t& msg); + static void set_input_current_callback(Axis* axis, can_Message_t& msg); + static void set_controller_modes_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); static void set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg); diff --git a/docs/can-protocol.md b/docs/can-protocol.md index fd1c49d1..fe24cab4 100644 --- a/docs/can-protocol.md +++ b/docs/can-protocol.md @@ -46,10 +46,10 @@ CMD ID | Name | Sender | Signals | Start byte 0x008 | Set Axis Startup Config | Master | - Not yet implemented - | - 0x009 | Get Encoder Estimates\* | Master | Encoder Pos Estimate
Encoder Vel Estimate | 0
4 0x00A | Get Encoder Count\* | Master | Encoder Shadow Count
Encoder Count in CPR | 0
4 -0x00B | Move To Pos | Master | Goal Position | 0 -0x00C | Set Pos Setpoint | Master | Pos Setpoint
Vel FF
Current FF | 0
4
6 -0x00D | Set Vel Setpoint | Master | Vel Setpoint
Current FF | 0
4 -0x00E | Set Current Setpoint | Master | Current Setpoint | 0 +0x00B | Set Controller Modes | Master | Control Mode
Input Mode | 0
4 +0x00C | Set Input Pos | Master | Input Pos
Vel FF
Current FF | 0
4
6 +0x00D | Set Input Vel | Master | Input Current
Current FF | 0
4 +0x00E | Set Input Current | Master | Input Current | 0 0x00F | Set Velocity Limit | Master | Velocity Limit | 0 0x010 | Start Anticogging | Master | - | - 0x011 | Set Traj Vel Limit | Master | Traj Vel Limit | 0 @@ -79,12 +79,13 @@ Encoder Pos Estimate | IEEE 754 Float | 32 | 1 | 0 | Intel Encoder Vel Estimate | IEEE 754 Float | 32 | 1 | 0 | Intel Encoder Shadow Count | Signed Int | 32 | 1 | 0 | Intel Encoder Count In CPR | Signed Int | 32 | 1 | 0 | Intel -Goal Position | Signed Int | 32 | 1 | 0 | Intel -Pos Setpoint | Signed Int | 32 | 1 | 0 | Intel +Control Mode | Signed Int | 32 | 1 | 0 | Intel +Input Mode | Signed Int | 32 | 1 | 0 | Intel +Input Pos | Signed Int | 32 | 1 | 0 | Intel Vel FF | Signed Int | 16 | 0.1 | 0 | Intel Current FF | Signed Int | 16 | 0.01 | 0 | Intel -Vel Setpoint | Signed Int | 32 | 0.01 | 0 | Intel -Current Setpoint | Signed Int | 32 | 0.01 | 0 | Intel +Input Vel | Signed Int | 32 | 0.01 | 0 | Intel +Input Current | Signed Int | 32 | 0.01 | 0 | Intel Velocity Limit | IEEE 754 Float | 32 | 1 | 0 | Intel Traj Vel Limit | IEEE 754 Float | 32 | 1 | 0 | Intel Traj Accel Limit | IEEE 754 Float | 32 | 1 | 0 | Intel