Merge pull request #502 from Wetmelon/set_linear_ascii

Add set_linear_count function to ASCII and CAN

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,37 @@ 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) {
+    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;
+
+        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;
 

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<int32_t>(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
-}
+}

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);

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 | - | -  | - | - | - | - | -
 -|-|-|----------------------------------|-|--------------------|-|-|-|_