From d24d9f8e1ce6a86f09fc1f03feaa161f1d53cc98 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 7 Oct 2020 22:43:11 -0400 Subject: [PATCH 1/3] Add set_linear_count function to ASCII and CAN --- Firmware/communication/ascii_protocol.cpp | 32 ++++++++++++++++++++++- Firmware/communication/can_simple.cpp | 6 ++++- Firmware/communication/can_simple.hpp | 1 + docs/can-protocol.md | 1 + 4 files changed, 38 insertions(+), 2 deletions(-) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index ae23a614..ed5b7ff7 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -45,6 +45,7 @@ void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_check void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum); void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum); void cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_encoder(char * pStr, StreamSink& response_channel, bool use_checksum); /* Function implementations --------------------------------------------------*/ @@ -122,6 +123,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& case 'r': cmd_read_property(cmd, response_channel, use_checksum); break; // read property case 'w': cmd_write_property(cmd, response_channel, use_checksum); break; // write property case 'u': cmd_update_axis_wdg(cmd, response_channel, use_checksum); break; // Update axis watchdog. + case 'e': cmd_encoder(cmd, response_channel, use_checksum); break; // Encoder commands default : cmd_unknown(nullptr, response_channel, use_checksum); break; } } @@ -220,11 +222,39 @@ void cmd_set_torque(char * pStr, StreamSink& response_channel, bool use_checksum } } +// @brief Sets the encoder linear count +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void cmd_encoder(char * pStr, StreamSink& response_channel, bool use_checksum) { + char firstChar = pStr[1]; + + if (firstChar == 's') { + memmove(pStr, pStr + 2, strlen(pStr + 2)+1); // Substring two characters to the right + + unsigned motor_number; + int encoder_count; + + if (sscanf(pStr, "l %u %i", &motor_number, &encoder_count) < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + Axis& axis = axes[motor_number]; + axis.encoder_.set_linear_count(encoder_count); + axis.watchdog_feed(); + respond(response_channel, use_checksum, "encoder set to %u", encoder_count); + } + } else { + respond(response_channel, use_checksum, "invalid command format"); + } +} + // @brief Executes the set trapezoid trajectory command // @param pStr buffer of ASCII encoded values // @param response_channel reference to the stream to respond on // @param use_checksum bool to indicate whether a checksum is required on response -void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum) { +void cmd_set_trapezoid_trajectory(char* pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float goal_point; diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index a6222cdc..8133e389 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -382,6 +382,10 @@ void CANSimple::clear_errors_callback(Axis* axis, can_Message_t& msg) { axis->clear_errors(); } +void CANSimple::set_linear_count_callback(Axis* axis, can_Message_t& msg){ + axis->encoder_.set_linear_count(can_getSignal(msg, 0, 32, true)); +} + void CANSimple::send_heartbeat(Axis* axis) { can_Message_t txmsg; txmsg.id = axis->config_.can_node_id << NUM_CMD_ID_BITS; @@ -409,4 +413,4 @@ uint32_t CANSimple::get_node_id(uint32_t msgID) { uint8_t CANSimple::get_cmd_id(uint32_t msgID) { return (msgID & 0x01F); // Bottom 5 bits -} \ No newline at end of file +} diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index 0168b978..aa5e6650 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -62,6 +62,7 @@ class CANSimple { static void get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg); static void get_vbus_voltage_callback(Axis* axis, can_Message_t& msg); static void clear_errors_callback(Axis* axis, can_Message_t& msg); + static void set_linear_count_callback(Axis* axis, can_Message_t& msg); // Utility functions static uint32_t get_node_id(uint32_t msgID); diff --git a/docs/can-protocol.md b/docs/can-protocol.md index 5ac54750..e9060277 100644 --- a/docs/can-protocol.md +++ b/docs/can-protocol.md @@ -60,6 +60,7 @@ CMD ID | Name | Sender | Signals | Start byte | Signal Type | Bits | Factor | Of 0x016 | Reboot ODrive | Master\*\*\* | - | - | - | - | - | - | - 0x017 | Get Vbus Voltage | Master\*\*\* | Vbus Voltage | 0 | IEEE 754 Float | 32 | 1 | 0 | Intel 0x018 | Clear Errors | Master | - | - | - | - | - | - | - +0x019 | Set Linear Count | Master | Position | 0 | Signed Int | 32 | 1 | 0 | Intel 0x700 | CANOpen Heartbeat Message\*\* | Slave | - | - | - | - | - | - | - -|-|-|----------------------------------|-|--------------------|-|-|-|_ From 7922f4f7ec862fe64855611f4c30608b2bd29b4b Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Fri, 9 Oct 2020 17:57:41 -0400 Subject: [PATCH 2/3] Fix substring --- Firmware/communication/ascii_protocol.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index ed5b7ff7..544fcf7d 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -230,7 +230,7 @@ void cmd_encoder(char * pStr, StreamSink& response_channel, bool use_checksum) { char firstChar = pStr[1]; if (firstChar == 's') { - memmove(pStr, pStr + 2, strlen(pStr + 2)+1); // Substring two characters to the right + pStr += 2; // Substring two characters to the right unsigned motor_number; int encoder_count; From 1720232563349a2fd39b6c622efa7d17e61ed3e9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 9 Oct 2020 15:24:24 -0700 Subject: [PATCH 3/3] Update ascii_protocol.cpp --- Firmware/communication/ascii_protocol.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 544fcf7d..3909f17c 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -227,10 +227,8 @@ void cmd_set_torque(char * pStr, StreamSink& response_channel, bool use_checksum // @param response_channel reference to the stream to respond on // @param use_checksum bool to indicate whether a checksum is required on response void cmd_encoder(char * pStr, StreamSink& response_channel, bool use_checksum) { - char firstChar = pStr[1]; - - if (firstChar == 's') { - pStr += 2; // Substring two characters to the right + if (pStr[1] == 's') { + pStr += 2; // Substring two characters to the right (ok because we have guaranteed null termination after all chars) unsigned motor_number; int encoder_count;