From 1e4b71b67bb2ba8ba0bd10135dd2db08f2d07def Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sat, 9 Feb 2019 21:48:30 -0800
Subject: [PATCH] add ascii command f for feedback

---
 Firmware/communication/ascii_protocol.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp
index 380530d1..3ac68499 100644
--- a/Firmware/communication/ascii_protocol.cpp
+++ b/Firmware/communication/ascii_protocol.cpp
@@ -155,6 +155,19 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
             axes[motor_number]->controller_.move_to_pos(goal_point);
         }
 
+    } else if (cmd[0] == 'f') { // feedback
+        unsigned motor_number;
+        int numscan = sscanf(cmd, "f %u", &motor_number);
+        if (numscan < 1) {
+            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 {
+            respond(response_channel, use_checksum, "%f %f",
+                    (double)axes[motor_number]->encoder_.pos_estimate_,
+                    (double)axes[motor_number]->encoder_.vel_estimate_);
+        }
+
     } else if (cmd[0] == 'h') {  // Help
         respond(response_channel, use_checksum, "Please see documentation for more details");
         respond(response_channel, use_checksum, "");