diff --git a/Firmware/Inc/usbd_cdc_if.h b/Firmware/Inc/usbd_cdc_if.h
index 8bf41d3c..3584347d 100644
--- a/Firmware/Inc/usbd_cdc_if.h
+++ b/Firmware/Inc/usbd_cdc_if.h
@@ -71,6 +71,10 @@
   * @{
   */ 
 /* USER CODE BEGIN EXPORTED_DEFINES */
+/* Define size for the receive and transmit buffer over CDC */
+/* It's up to user to redefine and/or remove those define */
+#define USB_RX_DATA_SIZE  64
+#define USB_TX_DATA_SIZE  64
 /* USER CODE END EXPORTED_DEFINES */
 
 /**
@@ -103,8 +107,6 @@
 extern USBD_CDC_ItfTypeDef  USBD_Interface_fops_FS;
 
 /* USER CODE BEGIN EXPORTED_VARIABLES */
-extern uint8_t *USBRxBuffer;
-extern uint32_t USBRxBufferLen;
 /* USER CODE END EXPORTED_VARIABLES */
 
 /**
diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp
index 10eb6a71..436889b6 100644
--- a/Firmware/MotorControl/commands.cpp
+++ b/Firmware/MotorControl/commands.cpp
@@ -2,7 +2,7 @@
 /* Includes ------------------------------------------------------------------*/
 
 // TODO: remove this option
-#define ENABLE_LEGACY_PROTOCOL
+//#define ENABLE_LEGACY_PROTOCOL
 
 #include "low_level.h"
 #include "protocol.hpp"
@@ -37,59 +37,104 @@ static const GpioMode_t gpio_mode = GPIO_MODE_UART;     //GPIO 1,2 is UART Tx,Rx
 /* Private variables ---------------------------------------------------------*/
 
 /* Variables exposed to USB & UART via read/write commands */
-//Oskar: what is range information?
 // TODO: include range information in JSON description
 
+std::function<void(void)> motors_0_set_pos_setpoint_func = std::bind(set_pos_setpoint, &motors[0],
+    std::ref(motors[0].set_pos_setpoint_args.pos_setpoint),
+    std::ref(motors[0].set_pos_setpoint_args.vel_feed_forward),
+    std::ref(motors[0].set_pos_setpoint_args.current_feed_forward)
+);
+std::function<void(void)> motors_0_set_vel_setpoint_func = std::bind(set_vel_setpoint, &motors[0],
+    std::ref(motors[0].set_vel_setpoint_args.vel_setpoint),
+    std::ref(motors[0].set_vel_setpoint_args.current_feed_forward)
+);
+std::function<void(void)> motors_0_set_current_setpoint_func = std::bind(set_current_setpoint, &motors[0],
+    std::ref(motors[0].set_current_setpoint_args.current_setpoint)
+);
+
 // clang-format off
-//Oskar: The endpoint table should be const, yeah? Then it can be put in RO memory (flash).
-Endpoint endpoints[] = {
-    Endpoint("vbus_voltage", const_cast<const float*>(&vbus_voltage)),
-    Endpoint("elec_rad_per_enc", const_cast<const float*>(&elec_rad_per_enc)),
-    Endpoint("motor0", BEGIN_TREE, nullptr, nullptr, nullptr),
-        Endpoint("pos_setpoint", &motors[0].pos_setpoint),
-        Endpoint("pos_gain", &motors[0].pos_gain),
-        Endpoint("vel_setpoint", &motors[0].vel_setpoint),
-    Endpoint(nullptr, END_TREE, nullptr, nullptr, nullptr) // motor0
+// TODO: Autogenerate this table. It will come up again very soon in the Arduino library.
+const Endpoint endpoints[] = {
+    Endpoint::make_property("vbus_voltage", const_cast<const float*>(&vbus_voltage)),
+    Endpoint::make_property("elec_rad_per_enc", const_cast<const float*>(&elec_rad_per_enc)),
+    Endpoint::make_object("motor0"),
+        Endpoint::make_property("pos_setpoint", &motors[0].pos_setpoint),
+        Endpoint::make_property("pos_gain", &motors[0].pos_gain),
+        Endpoint::make_property("vel_setpoint", &motors[0].vel_setpoint),
+        Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func),
+            Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint),
+            Endpoint::make_property("vel_feed_forward", &motors[0].set_pos_setpoint_args.vel_feed_forward),
+            Endpoint::make_property("current_feed_forward", &motors[0].set_pos_setpoint_args.current_feed_forward),
+        Endpoint::close_tree(),
+        Endpoint::make_function("set_vel_setpoint", &motors_0_set_vel_setpoint_func),
+            Endpoint::make_property("vel_setpoint", &motors[0].set_vel_setpoint_args.vel_setpoint),
+            Endpoint::make_property("current_feed_forward", &motors[0].set_vel_setpoint_args.current_feed_forward),
+        Endpoint::close_tree(),
+        Endpoint::make_function("set_current_setpoint", &motors_0_set_current_setpoint_func),
+            Endpoint::make_property("current_setpoint", &motors[0].set_current_setpoint_args.current_setpoint),
+        Endpoint::close_tree(),
+    Endpoint::close_tree() // motor0
 };
 // clang-format on
 
 constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]);
 
 
+
+//#define STREAM_ON_USB
+#ifdef STREAM_ON_USB
 // We could theoretically implement the USB channel as a packet based channel,
 // but on some platforms there's no direct USB endpoint access, so the device
 // should better just behave like a serial device.
 
-//Oskar: We should discuss this, possibly have both options.
-// On windows the serial driver seems really buggy...
-class USBSender : public StreamWriter {
+class USBSender : public StreamSink {
 public:
-    int write_bytes(const uint8_t* buffer, size_t length) {
+    int process_bytes(const uint8_t* buffer, size_t length) {
         // Loop to ensure all bytes get sent
         // TODO: add timeout
         while (length) {
-            size_t chunk = length < 64 ? length : 64;
+            size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
             while (CDC_Transmit_FS(
                 const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
                 well... it's not actually. Stupid STM. */, chunk) != USBD_OK)
                 osDelay(1);
             buffer += chunk;
-            length -= chunk;
+            length -= chunk;printf("got packet of length %d: \r\n", length); osDelay(5); hexdump(buffer, length);
         }
         //printf("USB TX done\r\n"); osDelay(5);
         return 0;
     }
+
+    size_t get_free_space() { return SIZE_MAX; }
+} usb_sender;
+
+PacketToStreamConverter usb_packet_sender(usb_sender);
+BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_packet_sender);
+StreamToPacketConverter usb_stream_sink(usb_connection);
+
+#else
+
+class USBSender : public PacketSink {
+public:
+    int process_packet(const uint8_t* buffer, size_t length) {
+        // cannot send partial packets
+        if (length > USB_TX_DATA_SIZE)
+            return -1;
+        while (CDC_Transmit_FS(
+            const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
+            well... it's not actually. Stupid STM. */, length) != USBD_OK)
+            osDelay(1);
+        //printf("USB TX done\r\n"); osDelay(5);
+        return 0;
+    }
 } usb_sender;
 
-PacketToStreamConverter usb_packet_sender(usb_sender);
-BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_packet_sender);
-StreamToPacketConverter usb_stream_writer(usb_connection);
+BidirectionalPacketBasedChannel usb_connection(endpoints, NUM_ENDPOINTS, usb_sender);
+#endif
 
-class UART4Sender : public StreamWriter {
-private:
-    uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
+class UART4Sender : public StreamSink {
 public:
-    int write_bytes(const uint8_t* buffer, size_t length) {
+    int process_bytes(const uint8_t* buffer, size_t length) {
         //Check length
         if (length > UART_TX_BUFFER_SIZE)
             return -1;
@@ -102,11 +147,15 @@ public:
         HAL_UART_Transmit_DMA(&huart4, tx_buf_, length);
         return 0;
     }
+
+    size_t get_free_space() { return SIZE_MAX; }
+private:
+    uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
 } uart4_sender;
 
 PacketToStreamConverter uart4_packet_sender(uart4_sender);
 BidirectionalPacketBasedChannel uart4_connection(endpoints, NUM_ENDPOINTS, uart4_packet_sender);
-StreamToPacketConverter UART4_stream_writer(uart4_connection);
+StreamToPacketConverter UART4_stream_sink(uart4_connection);
 
 /* Private function prototypes -----------------------------------------------*/
 /* Function implementations --------------------------------------------------*/
@@ -156,7 +205,7 @@ void communication_task(void const * argument) {
             uint8_t c = dma_circ_buffer[last_rcv_idx];
             if (++last_rcv_idx == UART_RX_BUFFER_SIZE)
                 last_rcv_idx = 0;
-            UART4_stream_writer.write_bytes(&c, 1);
+            UART4_stream_sink.process_bytes(&c, 1);
         }
 
         // When we reach here, we are out of immediate characters to fetch out of UART buffer
@@ -165,10 +214,10 @@ void communication_task(void const * argument) {
         int USB_check_timeout = 1;
         int32_t status = osSemaphoreWait(sem_usb_irq, USB_check_timeout);
         if (status == osOK) {
-            USB_receive_packet(USBRxBuffer, USBRxBufferLen);
-            // Allow receiving more bytes
-            USBD_CDC_SetRxBuffer(&hUsbDeviceFS, USBRxBuffer);
-            USBD_CDC_ReceivePacket(&hUsbDeviceFS);
+            // We have a new incoming USB transmission: handle it
+            HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
+            // Let the irq (OTG_FS_IRQHandler) fire again.
+            HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
         }
     }
 
@@ -189,5 +238,9 @@ void USB_receive_packet(const uint8_t *buffer, size_t length) {
     }
 #endif
     
-    usb_stream_writer.write_bytes(buffer, length);
+#ifdef STREAM_ON_USB
+    usb_stream_sink.process_bytes(buffer, length);
+#else
+    usb_connection.process_packet(buffer, length);
+#endif
 }
diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h
index 56cfa566..ade2f524 100644
--- a/Firmware/MotorControl/low_level.h
+++ b/Firmware/MotorControl/low_level.h
@@ -144,6 +144,20 @@ typedef struct {
     Sensorless_t sensorless;
     int timing_log_index;
     uint16_t timing_log[TIMING_LOG_SIZE];
+
+    // Cache for remote procedure calls arguments
+    struct {
+        float pos_setpoint; 
+        float vel_feed_forward;
+        float current_feed_forward;
+    } set_pos_setpoint_args;
+    struct {
+        float vel_setpoint;
+        float current_feed_forward;
+    } set_vel_setpoint_args;
+    struct {
+        float current_setpoint;
+    } set_current_setpoint_args;
 } Motor_t;
 
 typedef struct{
diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp
index 3fb1c3d4..41b80ccc 100644
--- a/Firmware/MotorControl/protocol.cpp
+++ b/Firmware/MotorControl/protocol.cpp
@@ -8,98 +8,80 @@
 #include <stdlib.h>
 
 /* Private defines -----------------------------------------------------------*/
+//#define DEGUG_PROTOCOL
 /* Private macros ------------------------------------------------------------*/
+
+#ifdef DEGUG_PROTOCOL
+#define LOG_PROTO(...)  do { printf(__VA_ARGS__); osDelay(10); } while (0)
+#else
+#define LOG_PROTO(...)  ((void) 0)
+#endif
+
 /* Private typedef -----------------------------------------------------------*/
 /* Global constant data ------------------------------------------------------*/
 /* Global variables ----------------------------------------------------------*/
 /* Private constant data -----------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
-
-// The order in this list must correspond to the order in EndpointTypeID_t
-//Oskar: Isn't it better to then have an array of tuples(or structs), so that they are always paired at definition?
-const char *type_names_[] = {
-    "json",
-    "int32[]",
-    "float",
-    "int",
-    "bool",
-    "uint16",
-    "tree"
-};
-
 /* Private function prototypes -----------------------------------------------*/
 
 static void hexdump(const uint8_t* buf, size_t len);
-static void write_buffer(const uint8_t* input, size_t input_length, size_t* skip, uint8_t** output, size_t* output_length);
-static void write_string(const char* str, size_t* skip, uint8_t** output, size_t* output_length);
+static inline int write_string(const char* str, StreamSink* output);
 
 /* Function implementations --------------------------------------------------*/
 
-// For debugging only
+#if 0
 void hexdump(const uint8_t* buf, size_t len) {
     for (size_t pos = 0; pos < len; ++pos) {
         printf(" %02x", buf[pos]);
         if ((((pos + 1) % 16) == 0) || ((pos + 1) == len))
             printf("\r\n");
-        osDelay(1);
+        osDelay(2);
     }
 }
+#else
+void hexdump(const uint8_t* buf, size_t len) {
+    (void) buf;
+    (void) len;
+}
+#endif
 
-// @brief Copies an input buffer to an output buffer, skipping a couple of bytes on the input buffer if required.
-// @param input: input buffer
-// @param input_length: number of bytes in the input buffer
-// @param skip: number of bytes to skip in the input buffer - will be set to max{skip - input_length, 0}
-// @param output: output buffer - will be increased by the number of bytes copied
-// @param output_length: length of the output buffer - will be decreased by the number of bytes copied
-void write_buffer(const uint8_t* input, size_t input_length, size_t* skip, uint8_t** output, size_t* output_length) {
-    if (*skip >= input_length) {
-        *skip -= input_length;
-    } else {
-        input_length -= *skip;
-        input += *skip;
-        *skip = 0;
-        size_t length = input_length < *output_length ? input_length : *output_length;
-        memcpy(*output, input, length);
-        *output += length;
-        *output_length -= length;
-    }
+static inline int write_string(const char* str, StreamSink* output) {
+    return output->process_bytes(reinterpret_cast<const uint8_t*>(str), strlen(str));
 }
 
-//Oskar: consier a JsonWriter object (as per slack discussion)
-static void write_string(const char* str, size_t* skip, uint8_t** output, size_t* output_length) {
-    write_buffer(reinterpret_cast<const uint8_t*>(str), strlen(str), skip, output, output_length);
-}
-
-void Endpoint::write_json(size_t id, size_t* skip, uint8_t** output, size_t* output_length, bool* need_comma) {
-    if (type_id_ == END_TREE) {
-        write_string("]}", skip, output, output_length);
+void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const {
+    if (type_ == CLOSE_TREE) {
+        write_string("]}", output);
         *need_comma = true;
-        return;
-    } else if (type_id_ < END_TREE) {
+    } else {
         if (*need_comma)
-            write_string(",", skip, output, output_length);
+            write_string(",", output);
 
-        write_string("{\"name\":\"", skip, output, output_length);
+        // write name
+        write_string("{\"name\":\"", output);
         if (name_)
-            write_string(name_, skip, output, output_length);
-        write_string("\",\"id\":", skip, output, output_length);
+            write_string(name_, output);
+
+        // write endpoint ID
+        write_string("\",\"id\":", output);
         char id_buf[10];
         snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
-        write_string(id_buf, skip, output, output_length);
-        write_string(",\"type\":\"", skip, output, output_length);
-        if (type_names_[type_id_])
-            write_string(type_names_[type_id_], skip, output, output_length);
-        write_string("\"", skip, output, output_length);
+        write_string(id_buf, output);
 
-        if (type_id_ == BEGIN_TREE) {
-            write_string(",\"content\":[", skip, output, output_length);
+        // write additional JSON data
+        if (json_modifier_ && json_modifier_[0]) {
+            write_string(",", output);
+            write_string(json_modifier_, output);
+        }
+
+        if (type_ == BEGIN_OBJECT) {
+            write_string(",\"content\":[", output);
             *need_comma = false;
-        } else {
-            if (json_modifier_ && json_modifier_[0]) {
-                write_string(",", skip, output, output_length);
-                write_string(json_modifier_, skip, output, output_length);
-            }
-            write_string("}", skip, output, output_length);
+        } else if (type_ == BEGIN_FUNCTION) {
+            write_string(",\"arguments\":[", output);
+            *need_comma = false;
+        } else if (type_ == PROPERTY) {
+            write_string("}", output);
             *need_comma = true;
         }
     }
@@ -107,7 +89,7 @@ void Endpoint::write_json(size_t id, size_t* skip, uint8_t** output, size_t* out
 
 
 
-int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) {
+int StreamToPacketConverter::process_bytes(const uint8_t *buffer, size_t length) {
     int result = 0;
 
     while (length--) {
@@ -121,7 +103,7 @@ int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) {
             } else if (header_index_ == 3 && calc_crc8(CRC8_INIT, header_buffer_, 3)) {
                 header_index_ = 0;
             } else if (header_index_ == 3) {
-                packet_length_ = header_buffer_[1];
+                packet_length_ = header_buffer_[1] + 2;
             }
         } else if (packet_index_ < sizeof(packet_buffer_)) {
             // Process payload byte
@@ -130,7 +112,9 @@ int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) {
 
         // If both header and packet are fully received, hand it on to the packet processor
         if (header_index_ == 3 && packet_index_ == packet_length_) {
-            result |= output_.write_packet(packet_buffer_, packet_length_);
+            if (calc_crc16(CRC16_INIT, packet_buffer_, packet_length_) == 0) {
+                result |= output_.process_packet(packet_buffer_, packet_length_ - 2);
+            }
             header_index_ = packet_index_ = packet_length_ = 0;
         }
         buffer++;
@@ -139,10 +123,12 @@ int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) {
     return result;
 }
 
-int PacketToStreamConverter::write_packet(const uint8_t *buffer, size_t length) {
+int PacketToStreamConverter::process_packet(const uint8_t *buffer, size_t length) {
+    // TODO: support buffer size >= 128
     if (length >= 128)
         return -1;
 
+    LOG_PROTO("send header\r\n");
     uint8_t header[] = {
         SYNC_BYTE,
         static_cast<uint8_t>(length),
@@ -150,86 +136,66 @@ int PacketToStreamConverter::write_packet(const uint8_t *buffer, size_t length)
     };
     header[2] = calc_crc8(CRC8_INIT, header, 2);
 
-    if (output_.write_bytes(header, sizeof(header)))
+    if (output_.process_bytes(header, sizeof(header)))
         return -1;
-    //printf("send payload:\r\n"); osDelay(5); hexdump(buffer, length);
-    if (output_.write_bytes(buffer, length))
+    LOG_PROTO("send payload:\r\n");
+    hexdump(buffer, length);
+    if (output_.process_bytes(buffer, length))
         return -1;
-    //osDelay(5); printf("sent!\r\n"); osDelay(5);
 
+    LOG_PROTO("send crc16\r\n");
+    uint16_t crc16 = calc_crc16(CRC16_INIT, buffer, length);
+    uint8_t crc16_buffer[] = {
+        (uint8_t)((crc16 >> 8) & 0xff),
+        (uint8_t)((crc16 >> 0) & 0xff)
+    };
+    if (output_.process_bytes(crc16_buffer, 2))
+        return -1;
+    LOG_PROTO("sent!\r\n");
     return 0;
 }
 
 
 // Calculates the CRC16 of the JSON interface descriptor.
-// Make sure this stays consistent with what interface_query returns.
 // The init value is the protocol version.
-
-//Oskar: consider non-loop version of calc_crc16 in write_buffer, as per slack discussion
 uint16_t BidirectionalPacketBasedChannel::calculate_json_crc16(void) {
-    uint8_t buffer[64];
-    size_t offset = 0;
-    bool need_comma = false;
+    CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
 
-    uint8_t *buffer_ptr = buffer;
-    size_t buffer_length = sizeof(buffer);
-    write_string("[", &offset, &buffer_ptr, &buffer_length);
-    uint16_t crc16 = calc_crc16(PROTOCOL_VERSION, buffer, sizeof(buffer) - buffer_length);
+    uint8_t offset[4] = { 0 };
+    interface_query(offset, sizeof(offset), &crc16_calculator);
 
-    for (size_t i = 0; i < n_endpoints_; ++i) {
-        buffer_ptr = buffer;
-        buffer_length = sizeof(buffer);
-        get_endpoint(i)->write_json(i, &offset, &buffer_ptr, &buffer_length, &need_comma);
-        crc16 = calc_crc16(crc16, buffer, sizeof(buffer) - buffer_length);
-    }
-
-    buffer_ptr = buffer;
-    buffer_length = sizeof(buffer);
-    write_string("]", &offset, &buffer_ptr, &buffer_length);
-    crc16 = calc_crc16(crc16, buffer, sizeof(buffer) - buffer_length);
-
-    return crc16;
+    return crc16_calculator.get_crc16();
 }
 
 // Returns part of the JSON interface definition.
-// Make sure this stays consistent with what calculate_json_crc16 calculates.
-// The init value is the protocol version.
-void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
+void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, StreamSink* output) {
     // The request must contain a 32 bit integer to specify an offset
     if (input_length < 4)
         return;
-    uint32_t offset32 = 0;
-    read_le<uint32_t>(&offset32, input);
-    size_t offset = offset32;
+    uint32_t offset = 0;
+    read_le<uint32_t>(&offset, input);
+    NullStreamSink output_with_offset = NullStreamSink(offset, *output);
     
     bool need_comma = false;
-    write_string("[", &offset, &output, output_length);
+    write_string("[", &output_with_offset);
     for (size_t i = 0; i < n_endpoints_; ++i) {
-        get_endpoint(i)->write_json(i, &offset, &output, output_length, &need_comma);
+        get_endpoint(i)->write_json(i, &need_comma, &output_with_offset);
+        if (!output->get_free_space())
+            return; // return early if the output cannot take more bytes
     }
-    write_string("]", &offset, &output, output_length);
+    write_string("]", &output_with_offset);
 }
 
 //Oskar: Can you please make a google sheet which describes the packet layout/format
-int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t length) {
-    //printf("got packet of length %d: \r\n", length); osDelay(5); hexdump(buffer, length);
+int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) {
+    LOG_PROTO("got packet of length %d: \r\n", length);
+    hexdump(buffer, length);
     if (length < 4)
         return -1;
 
-    // calculate CRC for later validation
-    uint16_t crc16 = calc_crc16(CRC16_INIT, buffer, length - 2);
-    uint8_t crc16_termination[] = {
-        (PROTOCOL_VERSION >> 0) & 0xff,
-        (PROTOCOL_VERSION >> 8) & 0xff,
-        buffer[length - 2],
-        buffer[length - 1]
-    };
-
     uint16_t seq_no = read_le<uint16_t>(&buffer, &length);
 
     if (seq_no & 0x8000) {
-        if (calc_crc16(crc16, crc16_termination, sizeof(crc16_termination)))
-            return -1;
         // TODO: ack handling
     } else {
         // TODO: think about some kind of ordering guarantees
@@ -239,46 +205,40 @@ int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t
         bool expect_response = endpoint_id & 0x8000;
         endpoint_id &= 0x7fff;
 
-        Endpoint* endpoint = get_endpoint(endpoint_id);
+        const Endpoint* endpoint = get_endpoint(endpoint_id);
         if (!endpoint)
             return -1;
 
-        // Verify packet CRC. The expected CRC termination value depends on the selected endpoint.
+        // Verify packet footer. The expected footer value depends on the selected endpoint.
         // For endpoint 0 this is just the protocol version, for all other endpoints it's a
         // CRC over the entire JSON descriptor tree (this may change in future versions).
-        if (endpoint_id) {
-            crc16_termination[0] = (json_crc_ >> 0) & 0xff;
-            crc16_termination[1] = (json_crc_ >> 8) & 0xff;
-        }
-        if (calc_crc16(crc16, crc16_termination, sizeof(crc16_termination))) {
-            //printf("crc16 for endpoint %d failed: expected termination %02x %02x\r\n", endpoint_id, crc16_termination[0], crc16_termination[1]); osDelay(5);
+        uint16_t expected_footer = endpoint_id ? json_crc_ : PROTOCOL_VERSION;
+        uint16_t actual_footer = buffer[length - 2] | (buffer[length - 1] << 8);
+        if (expected_footer != actual_footer) {
+            LOG_PROTO("footer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_footer, actual_footer);
             return -1;
         }
-        //printf("crc16 ok\r\n"); osDelay(5);
+        LOG_PROTO("footer ok\r\n");
 
         // TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
+
         uint16_t expected_response_length = read_le<uint16_t>(&buffer, &length);
 
-        // Let the endpoint do the processing
-        size_t requested_size = expected_response_length < (sizeof(tx_buf_) - 4) ? expected_response_length : (sizeof(tx_buf_) - 4);
-        size_t remaining_size = requested_size;
-        endpoint->handle(buffer, length - 2, tx_buf_ + 2, &remaining_size);
+        // Limit response length according to our local TX buffer size
+        if (expected_response_length > sizeof(tx_buf_) - 2)
+            expected_response_length = sizeof(tx_buf_) - 2;
+
+        MemoryStreamSink output(tx_buf_ + 2, expected_response_length);
+        endpoint->handle(buffer, length - 2, &output);
 
         // Send response
         if (expect_response) {
-            size_t tx_size = (requested_size - remaining_size) + 4;
+            size_t actual_response_length = expected_response_length - output.get_free_space() + 2;
             write_le<uint16_t>(seq_no | 0x8000, tx_buf_);
 
-            // Add protocol version for CRC calculation (overwritten by actual CRC)
-            tx_buf_[tx_size - 2] = (PROTOCOL_VERSION >> 0) & 0xff;
-            tx_buf_[tx_size - 1] = (PROTOCOL_VERSION >> 8) & 0xff;
-            crc16 = calc_crc16(CRC16_INIT, tx_buf_, tx_size);
-
-            // Append CRC in big endian
-            tx_buf_[tx_size - 2] = (crc16 >> 8) & 0xff;
-            tx_buf_[tx_size - 1] = (crc16 >> 0) & 0xff;
-
-            output_.write_packet(tx_buf_, tx_size);
+            LOG_PROTO("send packet:\r\n");
+            hexdump(tx_buf_, actual_response_length);
+            output_.process_packet(tx_buf_, actual_response_length);
         }
     }
 
diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp
index 38e6c775..17386219 100644
--- a/Firmware/MotorControl/protocol.hpp
+++ b/Firmware/MotorControl/protocol.hpp
@@ -26,14 +26,10 @@
 *   2. packet length (0-127, larger values are reserved)
 *   3. crc8(sync byte + packet length)
 *   4. packet (as per below)
+*   5. crc16(packet)
 *
 * ## Packet format: ##
 * (For instance USB)
-
-//Oskar: All packet formats I anticipate (USB, CAN, Ethernet) already have a transport
-// level CRC. So maybe we can throw out the packet level CRC check, and instead add
-// a data crc as step 5 on the stream format?
-
 *
 * __Request__
 *
@@ -41,14 +37,12 @@
 *   2. endpoint-id, MSB = "expect ack"
 *   3. expected_response_size
 *   4. payload (contains offset if required)
-*   5. crc16(seq-no + endpoint-id + payload + crc16(protocol_version + JSON))
-*      For endpoint 0 the JSON-CRC is not included
+*   5. crc16(protocol_version + JSON) or just protocol_version for endpoint 0
 *
 * __Response__
 *
 *   1. seq-no, MSB = 1
 *   2. payload
-*   3. crc16(seq-no + payload + protocol_version)
 *
 */
 
@@ -69,7 +63,10 @@ constexpr uint8_t SYNC_BYTE = '$';
 constexpr uint8_t CRC8_INIT = 0x42;
 constexpr uint16_t CRC16_INIT = 0x1337;
 constexpr uint16_t PROTOCOL_VERSION = 1;
+
+// This value must not be larger than USB_TX_DATA_SIZE defined in usbd_cdc_if.h
 constexpr uint16_t TX_BUF_SIZE = 32; // does not work with 64 for some reason
+constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently no effect because of protocol limitations
 
 
 template<typename T>
@@ -124,31 +121,162 @@ inline size_t read_le<float>(float* value, const uint8_t* buffer) {
     return read_le(reinterpret_cast<uint32_t*>(value), buffer);
 }
 
-
+// @brief Reads a value of type T from the buffer.
+// @param buffer    Pointer to the buffer to be read. The pointer is updated by the number of bytes that were read.
+// @param length    The number of available bytes in buffer. This value is updated to subtract the bytes that were read.
 template<typename T>
 static inline T read_le(const uint8_t** buffer, size_t* length) {
     T result;
     size_t cnt = read_le(&result, *buffer);
-    //Oskar: Is the style where you have a mutable length and buffer pointer
-    // a common form? I don't think I have seen it before: let me know if you have some 
-    // reference of this style.
-    // Maybe using iterators is better?
     *buffer += cnt;
     *length -= cnt;
     return result;
 }
 
 
+class PacketSink {
+public:
+    // @brief Processes a packet.
+    // @return: 0 on success, otherwise a non-zero error code
+    // TODO: define what happens when the output is congested. We can either drop the data or block.
+    // TODO: define what happens when the packet is larger than what the implementation can handle.
+    virtual int process_packet(const uint8_t* buffer, size_t length) = 0;
+};
+
+
+class StreamSink {
+public:
+    // @brief Processes a chunk of bytes that is part of a continuous stream.
+    // @return: 0 on success, otherwise a non-zero error code
+    // TODO: define what happens when the output is congested. We can either drop the data or block.
+    virtual int process_bytes(const uint8_t* buffer, size_t length) = 0;
+
+    // @brief Returns the number of bytes that can still be written to the stream.
+    // Shall return SIZE_MAX if the stream has unlimited lenght.
+    virtual size_t get_free_space() = 0;
+};
+
+
+class StreamToPacketConverter : public StreamSink {
+public:
+    StreamToPacketConverter(PacketSink& output) :
+        output_(output)
+    {
+    };
+
+    int process_bytes(const uint8_t *buffer, size_t length);
+    
+    size_t get_free_space() { return SIZE_MAX; }
+
+private:
+    uint8_t header_buffer_[3];
+    size_t header_index_ = 0;
+    uint8_t packet_buffer_[RX_BUF_SIZE];
+    size_t packet_index_ = 0;
+    size_t packet_length_ = 0;
+    PacketSink& output_;
+};
+
+
+class PacketToStreamConverter : public PacketSink {
+public:
+    PacketToStreamConverter(StreamSink& output) :
+        output_(output)
+    {
+    };
+    
+    int process_packet(const uint8_t *buffer, size_t length);
+
+private:
+    StreamSink& output_;
+};
+
+
+// Implements the StreamSink interface by writing into a fixed size
+// memory buffer.
+class MemoryStreamSink : public StreamSink {
+public:
+    MemoryStreamSink(uint8_t *buffer, size_t length) :
+        buffer_(buffer),
+        buffer_length_(length) {}
+
+    // Returns 0 on success and -1 if the buffer could not accept everything because it became full
+    int process_bytes(const uint8_t* buffer, size_t length) {
+        int status = 0;
+        if (length > buffer_length_) {
+            length = buffer_length_;
+            status = -1;
+        }
+        memcpy(buffer_, buffer, length);
+        buffer_ += length;
+        buffer_length_ -= length;
+        return status;
+    }
+
+    size_t get_free_space() { return buffer_length_; }
+
+private:
+    uint8_t * buffer_;
+    size_t buffer_length_;
+};
+
+// Implements the StreamSink interface by discarding the first couple of bytes
+// and then forwarding the rest to another stream.
+class NullStreamSink : public StreamSink {
+public:
+    NullStreamSink(size_t skip, StreamSink& follow_up_stream) :
+        skip_(skip),
+        follow_up_stream_(follow_up_stream) {}
+
+    // Returns 0 on success and -1 if the buffer could not accept everything because it became full
+    int process_bytes(const uint8_t* buffer, size_t length) {
+        if (skip_ < length) {
+            buffer += skip_;
+            length -= skip_;
+            skip_ = 0;
+            return follow_up_stream_.process_bytes(buffer, length);
+        } else {
+            skip_ -= length;
+            return 0;
+        }
+    }
+
+    size_t get_free_space() { return skip_ + follow_up_stream_.get_free_space(); }
+
+private:
+    size_t skip_;
+    StreamSink& follow_up_stream_;
+};
+
+
+
+// Implements the StreamSink interface by calculating the CRC16 checksum
+// on the data that is sent to it.
+class CRC16Calculator : public StreamSink {
+public:
+    CRC16Calculator(uint16_t crc16_init) :
+        crc16_(crc16_init) {}
+
+    int process_bytes(const uint8_t* buffer, size_t length) {
+        crc16_ = calc_crc16(crc16_, buffer, length);
+        return 0;
+    }
+
+    size_t get_free_space() { return SIZE_MAX; }
+
+    uint16_t get_crc16() { return crc16_; }
+private:
+    uint16_t crc16_;
+};
+
+
+
 typedef enum {
-    AS_JSON,
-    AS_INT32_ARRAY,
-    AS_FLOAT,
-    AS_INT,
-    AS_BOOL,
-    AS_UINT16,
-    BEGIN_TREE,
-    END_TREE
-} EndpointTypeID_t;
+    PROPERTY,
+    BEGIN_OBJECT,
+    BEGIN_FUNCTION,
+    CLOSE_TREE
+} EndpointType_t;
 
 
 // @brief Endpoint request handler
@@ -160,58 +288,67 @@ typedef enum {
 //
 // @param input: pointer to the input data
 // @param input_length: number of available input bytes
-// @param output: pointer to where the output data should go.
-//        If *output_length is non-zero, this is guaranteed not to be NULL.
-// @param output_length: pointer to the remaining length of the output buffer.
-//        The handler must update this value to subtract the number of written bytes.
-//        The pointer itself is guaranteed not to be NULL.
-
-//Oskar: Suggestion: return number of bytes written, and pass output_length by value.
-// This style is more consistent with stdio functions
-typedef std::function<void(void* ctx, const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length)> EndpointHandler;
+// @param output: The stream where to write the output to. Can be null.
+//                The handler shall abort as soon as the stream returns
+//                a non-zero error code on write.
+typedef std::function<void(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output)> EndpointHandler;
 
 
 template<typename T>
-void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
+void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
     const T* value = reinterpret_cast<const T*>(ctx);
     // If the old value was requested, call the corresponding little endian serialization function
-    if (*output_length) {
-        // uint8_t buffer[8]; // TODO: make buffer size dependent on the type
-        uint8_t buffer[sizeof(T)]; //Oskar: You can do this.
+    if (output) {
+        // TODO: make buffer size dependent on the type
+        uint8_t buffer[sizeof(T)];
         size_t cnt = write_le<T>(*value, buffer);
-        if (cnt > *output_length)
-            cnt = *output_length; //Oskar: I woudldn't clip like this, some types become
-            // very wrong when clipped little endian (like float). Just write nothing if it doesnt fit.
-        memcpy(output, buffer, cnt);
-        *output_length -= cnt;
+        if (cnt <= output->get_free_space())
+            output->process_bytes(buffer, cnt);
     }
 }
 
 template<typename T>
-void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
+void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
     T* value = reinterpret_cast<T*>(ctx);
     
     // Read the endpoint value into output
-    default_read_endpoint_handler<T>(ctx, input, input_length, output, output_length);
+    default_read_endpoint_handler<T>(ctx, input, input_length, output);
     
     // If a new value was passed, call the corresponding little endian deserialization function
-    if (input_length) {
-        uint8_t buffer[8] = { 0 }; // TODO: make buffer size dependent on the type
-        if (input_length > sizeof(buffer))
-            input_length = sizeof(buffer);
-        //Oskar: Why do we need to take a copy of the input buffer?
-        memcpy(buffer, input, input_length); // TODO: abort if not enough bytes received
-        read_le<T>(value, buffer);
-    }
+    uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type
+    if (input_length >= sizeof(buffer))
+        read_le<T>(value, input);
+}
+
+static void trigger_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
+    (void) input;
+    (void) input_length;
+    (void) output;
+    std::function<void(void)> function = reinterpret_cast<void(*)()>(ctx);
+    function();
+}
+
+
+template<typename T>
+static inline const char* get_default_json_modifier();
+
+template<>
+inline const char* get_default_json_modifier<const float>() {
+    return "\"type\":\"float\",\"access\":\"r\"";
+}
+
+template<>
+inline const char* get_default_json_modifier<float>() {
+    return "\"type\":\"float\",\"access\":\"rw\"";
 }
 
 class Endpoint {
 public:
     const char* const name_;
 
-    Endpoint(const char* name, EndpointTypeID_t type_id, EndpointHandler handler, const char* json_modifier, void *ctx) :
+    Endpoint(const char* name, EndpointType_t type, EndpointHandler handler, const char* json_modifier, void *ctx) :
         name_(name),
-        type_id_(type_id),
+        type_(type),
         handler_(handler),
         json_modifier_(json_modifier),
         ctx_(ctx)
@@ -219,94 +356,56 @@ public:
     }
 
     template<typename T>
-    Endpoint(const char* name, const T* ctx) :
-        Endpoint(name, AS_FLOAT, default_read_endpoint_handler<T>, "\"access\":\"r\"",
-        const_cast<T*>(ctx) /* it's safe to cast the const away here because we
-        know that the default_read_endpoint_handler immediately adds it back */) {}
+    static Endpoint make_property(const char* name, const T* ctx) {
+        return Endpoint(name, PROPERTY,
+            default_read_endpoint_handler<T>,
+            get_default_json_modifier<const T>(),
+            const_cast<T*>(ctx) /* it's safe to cast the const away here because we
+            know that the default_read_endpoint_handler immediately adds it back */);
+    }
 
     template<typename T>
-    Endpoint(const char* name, T* ctx) :
-        Endpoint(name, AS_FLOAT, default_readwrite_endpoint_handler<T>, "\"access\":\"rw\"", ctx) {}
+    static Endpoint make_property(const char* name, T* ctx) {
+        return Endpoint(name, PROPERTY,
+            default_readwrite_endpoint_handler<T>,
+            get_default_json_modifier<T>(), ctx);
+    }
+    
+    static Endpoint make_object(const char* name) {
+        return Endpoint(name, BEGIN_OBJECT, nullptr,
+            "\"type\":\"object\"", nullptr);
+    }
 
+    static Endpoint make_function(const char* name, std::function<void(void)>* function) {
+        typedef void(f_t)(void);
+        f_t* f = *function->target<f_t*>();
+        return Endpoint(name, BEGIN_FUNCTION, trigger_endpoint_handler,
+            "\"type\":\"function\"", reinterpret_cast<void*>(f));
+    }
 
-    void write_json(size_t id, size_t* skip, uint8_t** output, size_t* output_length, bool* need_comma);
+    static Endpoint close_tree() {
+        return Endpoint(nullptr, CLOSE_TREE, nullptr, nullptr, nullptr);
+    }
 
-    void handle(const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
+    void write_json(size_t id, bool* need_comma, StreamSink* output) const;
+
+    void handle(const uint8_t* input, size_t input_length, StreamSink* output) const {
         if (handler_)
-            return handler_(ctx_, input, input_length, output, output_length);
+            return handler_(ctx_, input, input_length, output);
     }
 
 private:
-    const EndpointTypeID_t type_id_;
+    const EndpointType_t type_;
     const EndpointHandler handler_;
     const char* json_modifier_;
     void* const ctx_;
 };
 
-//Oskar: Writer implies that it writes things, but that's not what this is;
-// you can write to it, but it doesn't write things.
 
-// # Mabye use the terminology Source and Sink?
-// # <stuff>Writer -> <stuff>Sink
-// # <stuff>Reader -> <stuff>Source
-// # write_<stuff> -> process_<stuff>
-// # read_<stuff>  -> get_<stuff>
 
-class PacketWriter {
+class BidirectionalPacketBasedChannel : public PacketSink {
 public:
-    // @brief Processes a packet.
-    //Oskar: What is the return value?
-    // TODO: define what happens when the output is congested. We can either drop the data or block.
-    // TODO: define what happens when the packet is larger than what the implementation can handle.
-    virtual int write_packet(const uint8_t* buffer, size_t length) = 0;
-};
-
-
-class StreamWriter {
-public:
-    // @brief Processes a chunk of bytes that is part of a continuous stream.
-    //Oskar: What is the return value?
-    // TODO: define what happens when the output is congested. We can either drop the data or block.
-    virtual int write_bytes(const uint8_t* buffer, size_t length) = 0;
-};
-
-
-class StreamToPacketConverter : public StreamWriter {
-public:
-    StreamToPacketConverter(PacketWriter& output) :
-        output_(output)
-    {
-    };
-
-    int write_bytes(const uint8_t *buffer, size_t length);
-
-private:
-    uint8_t header_buffer_[3];
-    size_t header_index_ = 0;
-    uint8_t packet_buffer_[128]; //Oskar: Use some constexpr config name, hardcoded numbers are not so nice
-    size_t packet_index_ = 0;
-    size_t packet_length_ = 0;
-    PacketWriter& output_;
-};
-
-
-class PacketToStreamConverter : public PacketWriter {
-public:
-    PacketToStreamConverter(StreamWriter& output) :
-        output_(output)
-    {
-    };
-    
-    int write_packet(const uint8_t *buffer, size_t length);
-
-private:
-    StreamWriter& output_;
-};
-
-
-class BidirectionalPacketBasedChannel : public PacketWriter {
-public:
-    BidirectionalPacketBasedChannel(Endpoint* endpoints, size_t n_endpoints, PacketWriter& output) :
+    BidirectionalPacketBasedChannel(const Endpoint* endpoints, size_t n_endpoints, PacketSink& output) :
         global_endpoints_(endpoints),
         n_endpoints_(NUM_CHANNEL_SPECIFIC_ENDPOINTS + n_endpoints),
         output_(output),
@@ -314,35 +413,29 @@ public:
     {
     }
 
-    int write_packet(const uint8_t* buffer, size_t length);
+    int process_packet(const uint8_t* buffer, size_t length);
 
 private:
     
     uint16_t calculate_json_crc16(void);
-    void interface_query(const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length);
+    void interface_query(const uint8_t* input, size_t input_length, StreamSink* output);
 
-    static void interface_query_handler(void* ctx, const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
-        reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->interface_query(input, input_length, output, output_length);
+    static void interface_query_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
+        reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->interface_query(input, input_length, output);
     }
     
-    static void subscription_handler(void* ctx, const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
-        reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->subscription(input, input_length, output, output_length);
+    static void subscription_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
+        reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->subscription(input, input_length, output);
     }
 
-    //ReceiveCallback c = BidirectionalPacketBasedChannel::receive_fetch_request_ex;
-    //Oskar: by channel_specific, do you mean protocol_internal/specific?
-    Endpoint channel_specific_endpoints_[2] = {
-        Endpoint("", AS_JSON, BidirectionalPacketBasedChannel::interface_query_handler, nullptr, this),
-        Endpoint("subscriptions", AS_INT32_ARRAY, BidirectionalPacketBasedChannel::subscription_handler, nullptr, this)
+    const Endpoint channel_specific_endpoints_[1] = {
+        Endpoint("", PROPERTY, BidirectionalPacketBasedChannel::interface_query_handler, "\"type\":\"json\",\"access\":\"rw\"", this),
+        //Endpoint("subscriptions", PROPERTY, BidirectionalPacketBasedChannel::subscription_handler, nullptr, this)
     };
     static constexpr size_t NUM_CHANNEL_SPECIFIC_ENDPOINTS = sizeof(channel_specific_endpoints_) / sizeof(channel_specific_endpoints_[0]);
 
-    Endpoint* global_endpoints_;
-    size_t n_endpoints_;
-    PacketWriter& output_;
-
     
-    Endpoint* get_endpoint(size_t index) {
+    const Endpoint* get_endpoint(size_t index) {
         if (index < NUM_CHANNEL_SPECIFIC_ENDPOINTS){
             return &channel_specific_endpoints_[index];
         } else if (index < n_endpoints_) {
@@ -352,16 +445,14 @@ private:
         }
     }
 
-    //Oskar: What is a subscription? Let's discuss on slack
-    void subscription(const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
+    void subscription(const uint8_t* input, size_t input_length, StreamSink* output) {
         // TODO: handle
         return;
     }
 
-    //Oskar: try to keep a consistent declaration ordering between functions and data.
-    // see: https://google.github.io/styleguide/cppguide.html#Declaration_Order
-
-    size_t expected_seq_no_ = 0;
+    const Endpoint * const global_endpoints_;
+    size_t n_endpoints_;
+    PacketSink& output_;
     uint8_t tx_buf_[TX_BUF_SIZE];
     const uint16_t json_crc_;
 };
diff --git a/Firmware/README.md b/Firmware/README.md
index 5498b4b5..56632913 100644
--- a/Firmware/README.md
+++ b/Firmware/README.md
@@ -101,11 +101,19 @@ If you prefer to debug from eclipse, see [Setting up Eclipse development environ
 There is currently a very primitive method to read/write configuration, commands and errors from the ODrive over the USB.
 Please use the `tools/test_communication.py` python script for this.  It is written for Python 3.
 
-Setup instructions as follows:
-* Install PyUSB (pip install --pre pysusb)
+* Assuming you already have Python, install dependencies:
+
+    pip install pyusb pyserial prompt_toolkit
+
+* __Linux__ set up USB permissions
+
+    echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d3[1-3]", MODE="0666"' | sudo tee /etc/udev/rules.d/50-odrive.rules
+    sudo udevadm control --reload-rules
+    sudo udevadm trigger # until you reboot you may need to do this everytime you reset the ODrive
+
 * Plug in the STLink or another power source to power the ODrive board
 * Plug in a separate USB cable into the microUSB connector on ODrive
-* On Windows, use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb
+* __Windows__ Use the [Zadig](http://zadig.akeo.ie/) utility to set ODrive (not STLink!) driver to libusb
 * Run `tools/test_communication.py`
 
 ### Command set
diff --git a/Firmware/Src/stm32f4xx_it.c b/Firmware/Src/stm32f4xx_it.c
index 14128ee7..a740268a 100644
--- a/Firmware/Src/stm32f4xx_it.c
+++ b/Firmware/Src/stm32f4xx_it.c
@@ -266,6 +266,13 @@ void OTG_FS_IRQHandler(void)
 {
   /* USER CODE BEGIN OTG_FS_IRQn 0 */
 
+  // Mask interrupt, and signal processing of interrupt by usb_cmd_thread
+  // The thread will re-enable the interrupt when all pending irqs are clear.
+  HAL_NVIC_DisableIRQ(OTG_FS_IRQn);
+  osSemaphoreRelease(sem_usb_irq);
+  // Bypass interrupt processing here
+  return;
+
   /* USER CODE END OTG_FS_IRQn 0 */
   HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
   /* USER CODE BEGIN OTG_FS_IRQn 1 */
diff --git a/Firmware/Src/usbd_cdc_if.c b/Firmware/Src/usbd_cdc_if.c
index 48c7151f..f7dff345 100644
--- a/Firmware/Src/usbd_cdc_if.c
+++ b/Firmware/Src/usbd_cdc_if.c
@@ -77,10 +77,6 @@
   * @{
   */ 
 /* USER CODE BEGIN PRIVATE_DEFINES */
-/* Define size for the receive and transmit buffer over CDC */
-/* It's up to user to redefine and/or remove those define */
-#define APP_RX_DATA_SIZE  64
-#define APP_TX_DATA_SIZE  64
 /* USER CODE END PRIVATE_DEFINES */
 /**
   * @}
@@ -102,10 +98,10 @@
 /* Create buffer for reception and transmission           */
 /* It's up to user to redefine and/or remove those define */
 /* Received Data over USB are stored in this buffer       */
-uint8_t UserRxBufferFS[APP_RX_DATA_SIZE];
+uint8_t UserRxBufferFS[USB_RX_DATA_SIZE];
 
 /* Send Data over USB CDC are stored in this buffer       */
-uint8_t UserTxBufferFS[APP_TX_DATA_SIZE];
+uint8_t UserTxBufferFS[USB_TX_DATA_SIZE];
 
 /* USER CODE BEGIN PRIVATE_VARIABLES */
 /* USER CODE END PRIVATE_VARIABLES */
@@ -119,8 +115,6 @@ uint8_t UserTxBufferFS[APP_TX_DATA_SIZE];
   */ 
   extern USBD_HandleTypeDef hUsbDeviceFS;
 /* USER CODE BEGIN EXPORTED_VARIABLES */
-uint8_t *USBRxBuffer = UserRxBufferFS;
-uint32_t USBRxBufferLen;
 /* USER CODE END EXPORTED_VARIABLES */
 
 /**
@@ -273,13 +267,12 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
 {
   /* USER CODE BEGIN 6 */
 
-  // unblock the processing thread
-  USBRxBufferLen = *Len;
-  osSemaphoreRelease(sem_usb_irq);
+  // Process command
+  USB_receive_packet(Buf, *Len);
 
-  //Oskar: You shouldn't return from this function until you have copied data out of Buf,
-  // see the @note above.
-  // So I would revert to the tread-deffered USB irq processing, and call USB_receive_packet from here.
+  // Allow receiving more bytes
+  USBD_CDC_SetRxBuffer(&hUsbDeviceFS, UserRxBufferFS);
+  USBD_CDC_ReceivePacket(&hUsbDeviceFS);
 
   // once the data is handled, the processing thread will start the next transmission
   return (USBD_OK);
@@ -303,7 +296,7 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len)
   /* USER CODE BEGIN 7 */ 
   
   //Check length
-  if (Len > APP_TX_DATA_SIZE)
+  if (Len > USB_TX_DATA_SIZE)
     return USBD_FAIL;
   // Check for ongoing transmission
   USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*) hUsbDeviceFS.pClassData;
diff --git a/tools/demo.py b/tools/demo.py
index b6d15430..808d46c6 100755
--- a/tools/demo.py
+++ b/tools/demo.py
@@ -27,7 +27,7 @@ print("Position setpoint is " + str(my_drive.motor0.pos_setpoint))
 
 # little sine wave to test
 t0 = time.monotonic()
-while False:
+while True:
     setpoint = 10000.0 * math.sin((time.monotonic() - t0)*2)
     print("goto " + str(int(setpoint)))
     my_drive.motor0.pos_setpoint = setpoint
diff --git a/tools/odrive/core.py b/tools/odrive/core.py
index 4741ec28..c8787ce4 100644
--- a/tools/odrive/core.py
+++ b/tools/odrive/core.py
@@ -10,6 +10,7 @@ import usb.util
 import odrive.usbbulk
 import odrive.mock_device
 import odrive.util
+import odrive.usbbulk
 import re
 import serial
 import time
@@ -45,7 +46,7 @@ class SimpleDeviceProperty(property):
         self._channel.remote_endpoint_operation(self._id, buffer, True, 0)
 
 
-def create_object(json_data, namespace, channel):
+def create_object(json_data, namespace, channel, printer=noprint):
     """
     Creates an object that implements the specified JSON type description by
     communicating with the provided device object
@@ -56,16 +57,16 @@ def create_object(json_data, namespace, channel):
     for item in json_data:
         name = item.get("name", None)
         if name is None:
-            sys.stderr.write("unnamed property in {}".format(namespace))
+            printer("unnamed property in {}".format(namespace))
             continue
 
         type_str = item.get("type", None)
         if type_str is None:
-            sys.stderr.write("property {} has no specified type".format(name))
+            printer("property {} has no specified type".format(name))
             continue
 
-        if type_str == "tree":
-            properties[name] = create_object(item["content"], namespace + "." + item["name"], channel)
+        if type_str == "object":
+            properties[name] = create_object(item["content"], namespace + "." + item["name"], channel, printer=printer)
         else:
             if type_str == "float":
                 property_type = float
@@ -80,12 +81,12 @@ def create_object(json_data, namespace, channel):
                 property_type = int
                 struct_format = "<H"
             else:
-                sys.stderr.write("property {} has unsupported type {}".format(name, type_str))
+                printer("property {} has unsupported type {}".format(name or "[anonymous]", type_str))
                 continue
 
             id_str = item.get("id", None)
             if id_str is None:
-                sys.stderr.write("property {} has specified ID".format(name))
+                printer("property {} has specified ID".format(name))
                 continue
 
             access_mode = item.get("mode", "rw")
@@ -99,14 +100,14 @@ def create_object(json_data, namespace, channel):
     new_object = jit_type()
     return new_object
 
-class SerialDevice(odrive.protocol.StreamReader, odrive.protocol.StreamWriter):
+class SerialDevice(odrive.protocol.StreamSource, odrive.protocol.StreamSink):
     def __init__(self, port, baud):
         self._dev = serial.Serial(port, baud, timeout=1)
 
-    def write_bytes(self, bytes):
+    def process_bytes(self, bytes):
         self._dev.write(bytes)
 
-    def read_bytes(self, n_bytes, deadline):
+    def get_bytes(self, n_bytes, deadline):
         """
         Returns n bytes unless the deadline is reached, in which case the bytes
         that were read up to that point are returned. If deadline is None the
@@ -119,8 +120,8 @@ class SerialDevice(odrive.protocol.StreamReader, odrive.protocol.StreamWriter):
             self._dev.timeout = max(deadline - time.monotonic(), 0)
         return self._dev.read(n_bytes)
 
-    def read_bytes_or_fail(self, n_bytes, deadline):
-        result = self.read_bytes(n_bytes, deadline)
+    def get_bytes_or_fail(self, n_bytes, deadline):
+        result = self.get_bytes(n_bytes, deadline)
         if len(result) < n_bytes:
             raise odrive.protocol.TimeoutException()
         return result
@@ -137,12 +138,17 @@ def find_usb_channels(vid_pid_pairs=odrive.util.USB_VID_PID_PAIRS, printer=nopri
             continue
         printer("Found ODrive via PyUSB")
         bulk_device = odrive.usbbulk.USBBulkDevice(usb_device, printer)
-        printer(bulk_device.info())
-        bulk_device.init(printer)
-        #Oskar: Here bulk_device is expected to have a write_packet function, but it doesnt, it has read/write_bytes.
-        # We can either also use a stream converter here, but I'd prefer to write packets directly.
+        try:
+            printer(bulk_device.info())
+            bulk_device.init(printer)
+        except usb.core.USBError as ex:
+            if ex.errno == 13:
+                printer("USB device access denied. Did you set up your udev rules correctly?")
+                continue
+            else:
+                raise
         yield odrive.protocol.Channel(
-                "USB device {}:{}".format(vid_pid_pair[0], vid_pid_pair[1]),
+                "USB device bus {} device {}".format(usb_device.bus, usb_device.address),
                 bulk_device, bulk_device)
 
 def find_serial_channels(printer=noprint):
@@ -162,11 +168,11 @@ def find_serial_channels(printer=noprint):
         except serial.serialutil.SerialException:
             printer("could not open " + serial_port)
             continue
-        input = odrive.protocol.PacketFromStreamConverter(serial_device)
-        output = odrive.protocol.PacketToStreamConverter(serial_device)
+        input_stream = odrive.protocol.PacketFromStreamConverter(serial_device)
+        output_stream = odrive.protocol.PacketToStreamConverter(serial_device)
         yield odrive.protocol.Channel(
                 "serial port {}@{}".format(serial_port, 115200),
-                input, output)
+                input_stream, output_stream)
 
 
 def find_all(printer=noprint):
@@ -186,19 +192,19 @@ def find_all(printer=noprint):
             printer("no response - probably incompatible")
             continue
         json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes)
-        channel._interface_definition_crc = struct.pack("<H", json_crc16)
+        channel._interface_definition_crc = json_crc16
         try:
             json_string = json_bytes.decode("ascii")
         except UnicodeDecodeError:
             printer("device responded on endpoint 0 with something that is not ASCII")
             continue
-        #printer("JSON: " + json_string)
+        printer("JSON: " + json_string)
         try:
             json_data = json.loads(json_string)
         except json.decoder.JSONDecodeError:
             printer("device responded on endpoint 0 with something that is not JSON")
             continue
-        yield create_object(json_data, "odrive", channel)
+        yield create_object(json_data, "odrive", channel, printer=printer)
 
 
 def find_any(printer=noprint):
diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py
index 42823823..d7f239f2 100644
--- a/tools/odrive/protocol.py
+++ b/tools/odrive/protocol.py
@@ -11,7 +11,8 @@ PROTOCOL_VERSION = 1
 CRC8_DEFAULT = 0x37 # this must match the polynomial in the C++ implementation
 CRC16_DEFAULT = 0x3d65 # this must match the polynomial in the C++ implementation
 
-#Oskar: There must be a crc library for python already?
+MAX_PACKET_SIZE = 128
+
 def calc_crc(remainder, value, polynomial, bitwidth):
     topbit = (1 << (bitwidth - 1))
 
@@ -52,29 +53,20 @@ class TimeoutException(Exception):
 class ChannelBrokenException(Exception):
     pass
 
-#Oskar: Do these abstract classes even do anything when empty like this?
-# I'm not even too sure how this works in python...
-class StreamReader(object):
+class StreamSource(object):
     pass
 
-class StreamWriter(object):
+class StreamSink(object):
     pass
 
-class PacketReader(object):
+class PacketSource(object):
     pass
 
-class PacketWriter(object):
+class PacketSink(object):
     pass
 
 
-#Oskar: "StreamWriter" implies that it writes streams, but clearly it takes in ("reads") streams.
-# Mabye use the terminology Source and Sink?
-# <stuff>Writer -> <stuff>Sink
-# <stuff>Reader -> <stuff>Source
-# write_<stuff> -> process_<stuff>
-# read_<stuff>  -> get_<stuff>
-# Same comment for all the uses of the abstract classes.
-class StreamToPacketConverter(StreamWriter):
+class StreamToPacketConverter(StreamSink):
     _header = []
     _packet = []
     _packet_length = 0
@@ -82,11 +74,10 @@ class StreamToPacketConverter(StreamWriter):
     def __init__(self, output):
         self._output = output
 
-#Oskar: process_bytes?
-    def write_bytes(self, bytes):
+    def process_bytes(self, bytes):
         """
         Processes an arbitrary number of bytes. If one or more full packets are
-        are received, they are sent to this instance's output PacketWriter.
+        are received, they are sent to this instance's output PacketSink.
         Incomplete packets are buffered between subsequent calls to this function.
         """
         result = None
@@ -102,45 +93,50 @@ class StreamToPacketConverter(StreamWriter):
                 elif (len(self._header) == 3) and calc_crc8(CRC8_INIT, self._header):
                     self._header = []
                 elif (len(self._header) == 3):
-                    self._packet_length = self._header[1]
+                    self._packet_length = self._header[1] + 2
             else:
                 # Process payload byte
                 self._packet.append(byte)
 
             # If both header and packet are fully received, hand it on to the packet processor
             if (len(self._header) == 3) and (len(self._packet) == self._packet_length):
-                try:
-                    self._output.write_packet(self._packet)
-                except Exception as ex:
-                    result = ex
+                if calc_crc16(CRC16_INIT, self._packet) == 0:
+                    try:
+                        self._output.process_packet(self._packet[:-2])
+                    except Exception as ex:
+                        result = ex
                 self._header = []
                 self._packet = []
                 self._packet_length = 0
 
         if isinstance(result, Exception):
-            #Oskar: why are we removing exception information? Just let the original exception go up?
-            raise Exception("something went wrong")
+            # TODO: check if this is valid code (pylint complains)
+            raise result
 
 
-class PacketToStreamConverter(PacketWriter):
+class PacketToStreamConverter(PacketSink):
     def __init__(self, output):
         self._output = output
 
-    def write_packet(self, packet):
-        if (len(packet) >= 128): #Oskar: Use a config variable at top of file or in other file, hardcodes inline in code is hard to maintain.
+    def process_packet(self, packet):
+        if (len(packet) >= MAX_PACKET_SIZE):
             raise NotImplementedError("packet larger than 127 currently not supported")
 
         header = [SYNC_BYTE, len(packet)]
         header.append(calc_crc8(CRC8_INIT, header))
 
-        self._output.write_bytes(header)
-        self._output.write_bytes(packet)
+        self._output.process_bytes(header)
+        self._output.process_bytes(packet)
 
-class PacketFromStreamConverter(PacketReader, StreamWriter): #Oskar: This shouldn't inherit "StreamWriter", since it doesn't write_bytes.
+        # append CRC in big endian
+        crc16 = calc_crc16(CRC16_INIT, packet)
+        self._output.process_bytes(struct.pack('>H', crc16))
+
+class PacketFromStreamConverter(PacketSource):
     def __init__(self, input):
         self._input = input
     
-    def read_packet(self, deadline):
+    def get_packet(self, deadline):
         """
         Requests bytes from the underlying input stream until a full packet is
         received or the deadline is reached, in which case None is returned. A
@@ -150,29 +146,29 @@ class PacketFromStreamConverter(PacketReader, StreamWriter): #Oskar: This should
             header = bytes()
 
             # TODO: sometimes this call hangs, even though the device apparently sent something
-            header = header + self._input.read_bytes_or_fail(1, deadline)
+            header = header + self._input.get_bytes_or_fail(1, deadline)
             if (header[0] != SYNC_BYTE):
                 #print("sync byte mismatch")
                 continue
 
-            header = header + self._input.read_bytes_or_fail(1, deadline)
+            header = header + self._input.get_bytes_or_fail(1, deadline)
             if (header[1] & 0x80):
                 #print("packet too large")
                 continue # TODO: support packets larger than 128 bytes
 
-            header = header + self._input.read_bytes_or_fail(1, deadline)
+            header = header + self._input.get_bytes_or_fail(1, deadline)
             if calc_crc8(CRC8_INIT, header) != 0:
                 #print("crc8 mismatch")
                 continue
 
             packet_length = header[1]
             #print("wait for {} bytes".format(packet_length))
-            return self._input.read_bytes_or_fail(packet_length, deadline)
+            return self._input.get_bytes_or_fail(packet_length, deadline)
 
 
-class Channel(PacketWriter):
+class Channel(PacketSink):
     _outbound_seq_no = 0
-    _interface_definition_crc = bytearray(2)
+    _interface_definition_crc = 0
     _expected_acks = {}
 
     # Chose these parameters to be sensible for a specific transport layer
@@ -182,10 +178,10 @@ class Channel(PacketWriter):
     def __init__(self, name, input, output):
         """
         Params:
-        input: A PacketReader where this channel will source packets from on
+        input: A PacketSource where this channel will source packets from on
                demand. Alternatively packets can be provided to this channel
-               directly by calling write_packet on this instance.
-        output: A PacketWriter where this channel will put outgoing packets.
+               directly by calling process_packet on this instance.
+        output: A PacketSink where this channel will put outgoing packets.
         """
         self._name = name
         self._input = input
@@ -207,30 +203,27 @@ class Channel(PacketWriter):
 
         crc16 = calc_crc16(CRC16_INIT, packet)
         if (endpoint_id & 0x7fff == 0):
-            #print("append crc16 for " + str(struct.pack('<H', PROTOCOL_VERSION)))
-            crc16 = calc_crc16(crc16, struct.pack('<H', PROTOCOL_VERSION))
+            footer = PROTOCOL_VERSION
         else:
-            #print("append crc16 for " + str(self._interface_definition_crc))
-            crc16 = calc_crc16(crc16, self._interface_definition_crc)
-
-        # append CRC in big endian
-        packet = packet + struct.pack('>H', crc16)
+            footer = self._interface_definition_crc
+        #print("append footer " + footer)
+        packet = packet + struct.pack('<H', footer)
 
         if (expect_ack):
             self._expected_acks[seq_no] = None
             attempt = 0
             while (attempt < self._send_attempts):
-                self._output.write_packet(packet)
+                self._output.process_packet(packet)
                 deadline = time.monotonic() + self._resend_delay
                 # Read and process packets until we get an ack or need to resend
                 # TODO: support I/O driven reception (wait on semaphore)
                 while True:
                     try:
-                        response = self._input.read_packet(deadline)
+                        response = self._input.get_packet(deadline)
                     except TimeoutException:
                         break # resend
                     # process response, which is hopefully our ACK
-                    self.write_packet(response)
+                    self.process_packet(response)
                     if not self._expected_acks[seq_no] is None:
                         return self._expected_acks.pop(seq_no, None)
                     break
@@ -239,7 +232,7 @@ class Channel(PacketWriter):
             raise ChannelBrokenException()
         else:
             # fire and forget
-            self._output.write_packet(packet)
+            self._output.process_packet(packet)
             return None
     
     def remote_endpoint_read_buffer(self, endpoint_id):
@@ -256,23 +249,20 @@ class Channel(PacketWriter):
             buffer += chunk
         return buffer
 
-    def write_packet(self, packet):
+    def process_packet(self, packet):
         #print("process packet")
-        if (len(packet) < 4):
+        packet = bytes(packet)
+        if (len(packet) < 2):
             raise Exception("packet too short")
 
-        # calculate CRC for later validation
-        crc16 = calc_crc16(CRC16_INIT, packet[:-2])
-
         seq_no = struct.unpack('<H', packet[0:2])[0]
 
         if (seq_no & 0x8000):
-            if (calc_crc16(crc16, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
-                raise Exception("CRC16 mismatch")
-
             seq_no &= 0x7fff
-            self._expected_acks[seq_no] = packet[2:-2]
+            self._expected_acks[seq_no] = packet[2:]
 
         else:
+            #if (calc_crc16(crc16, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
+            #    raise Exception("CRC16 mismatch")
             print("endpoint requested")
             # TODO: handle local endpoint operation
diff --git a/tools/odrive/usbbulk.py b/tools/odrive/usbbulk.py
index 3f3abae1..07ba62d9 100644
--- a/tools/odrive/usbbulk.py
+++ b/tools/odrive/usbbulk.py
@@ -5,19 +5,16 @@ import usb.core
 import usb.util
 import sys
 import odrive.protocol
+import time
 
 def noprint(x):
   pass
 
-# Even though USB is packet based, we do stream based communication because some
-# systems don't allow direct access to the USB endpoints, in which case the
-# device has to behave like a serial device.
+# TODO: Even though USB is packet based, we might wanna do stream based
+# communication because some systems don't allow direct access to the USB
+# endpoints, in which case the device has to behave like a serial device.
 
-#Oskar: If we are using stream based comms over USB, we shouldn't be detach_kernel_driver and epr.read,
-# we should be using /dev/ttyACM0 with system read/writes.
-# Actually, we should discuss the approach here, we need to decide if we should do packet based or not.
-
-class USBBulkDevice(odrive.protocol.PacketReader, odrive.protocol.PacketWriter):
+class USBBulkDevice(odrive.protocol.PacketSource, odrive.protocol.PacketSink):
   def __init__(self, dev, printer=noprint):
     self.dev = dev
     self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct)
@@ -74,14 +71,27 @@ class USBBulkDevice(odrive.protocol.PacketReader, odrive.protocol.PacketWriter):
   def shutdown(self):
     return 0
 
-#Oskar: I would prefer the raw USB access version to write packets directly instead of using a steram converter.
-  def write_bytes(self, usbBuffer):
-    ret = self.epw.write(usbBuffer, 0)
-    return ret
+  def process_packet(self, usbBuffer):
+    try:
+      ret = self.epw.write(usbBuffer, 0)
+      return ret
+    except usb.core.USBError as ex:
+      if ex.errno == 19: # "no such device"
+        raise odrive.protocol.ChannelBrokenException()
+      else:
+        raise
 
-  def read_bytes(self, bufferLen):
-    ret = self.epr.read(bufferLen, 100)
-    return ret
+  def get_packet(self, deadline):
+    try:
+      bufferLen = self.epr.wMaxPacketSize
+      timeout = max(int((deadline - time.monotonic()) * 1000), 0)
+      ret = self.epr.read(bufferLen, timeout)
+      return ret
+    except usb.core.USBError as ex:
+      if ex.errno == 19: # "no such device"
+        raise odrive.protocol.ChannelBrokenException()
+      else:
+        raise
 
   def send_max(self):
     return 64
diff --git a/tools/test_communication.py b/tools/test_communication.py
index 4758af95..c619a472 100755
--- a/tools/test_communication.py
+++ b/tools/test_communication.py
@@ -4,6 +4,11 @@ import argparse
 
 def parse_args():
   parser = argparse.ArgumentParser(description='Talk to a ODrive board over USB bulk channel.')
+  parser.add_argument("-v", "--verbose", action="store_true",
+                      help="print debug information")
+  parser.add_argument("-d", "--device", action="store",
+                      help="Specifies the device to talk to. If this parameter is provided but the device is not available, I will exit immediately."
+                      "If not provided I will do my best to find an ODrive on USB and serial ports.")
   return parser.parse_args()
 
 if __name__ == '__main__':
@@ -13,14 +18,61 @@ if __name__ == '__main__':
 import sys
 import time
 import threading
-from odrive import usbbulk
+import prompt_toolkit
+import re
+import tempfile
+import odrive.core
 
-running = True
-ready   = False
+
+def noprint(str):
+  pass
+
+def command_prompt_loop(device, history):
+  """
+  Presents the command prompt indefinitely until something goes wrong
+  """
+  # Load all motors
+  motors = []
+  if "motor0" in dir(device):
+    motors.append(device.motor0)
+  if "motor1" in dir(device):
+    motors.append(device.motor1)
+
+  print("Connected - have {} {}".format(len(motors), "motor" if len(motors) == 1 else "motors"))
+
+  while True:
+    try:
+      command = prompt_toolkit.prompt(
+        "ODrive> ",
+        history=history).strip()
+    except EOFError:
+      command = "exit"
+
+    if len(command) == 0:
+      continue
+    elif command.startswith("p "):
+      args = command[2:].split()
+      try:
+        motor = motors[int(args[0])]
+        pos = float(args[1])
+        vel = float(args[2])
+        cur = float(args[3])
+      except (ValueError, IndexError):
+        print("invalid command format")
+        continue
+      motor.pos_setpoint = pos
+    elif command == "q" or command == 'exit':
+      sys.exit()
+    else:
+      print("unknown command \"" + command + "\"")
 
 def main(args):
-  global running
-  print("ODrive USB Bulk Communications")
+  if (args.verbose):
+    printer = print
+  else:
+    printer = noprint
+
+  print("ODrive Control Utility")
   print("---------------------------------------------------------------------")
   print("USAGE:")
   print("\tPOSITION_CONTROL:\n\t\tp MOTOR_NUMBER POSITION VELOCITY CURRENT")
@@ -28,38 +80,24 @@ def main(args):
   print("\tCURRENT_CONTROL:\n\t\tc MOTOR_NUMBER CURRENT")
   print("\tQuit Python Script:\n\t\tq")
   print("---------------------------------------------------------------------")
-  # query device
-  dev = usbbulk.poll_odrive_bulk_device(printer=print)
-  print (dev.info())
-  print (dev.init())
-  # thread
-  thread = threading.Thread(target=receive_thread, args=[dev])
-  thread.start()
-  while running:
-    time.sleep(0.1)
+
+  history = prompt_toolkit.history.InMemoryHistory()
+
+  while True:
+    # Connect to device
+    if (args.device is None):
+      print("Waiting for device...")
+      device = odrive.core.find_any(printer=printer)
+    else:
+      device = odrive.core.open(args.device, printer=printer)
+
     try:
-      command = input("Enter ODrive command:\n")
-      if 'q' in command:
-        running = False
+      command_prompt_loop(device, history)
+    except odrive.protocol.ChannelBrokenException:
+      print("ODrive disconnected")
+      if not args.device is None:
         sys.exit()
-      else:
-        dev.send(command)
-    except:
-      running = False
-
-def receive_thread(dev):
-  global ready
-
-  while running:
-    time.sleep(0.1)
-    try:
-      message = dev.receive(dev.receive_max())
-      message_ascii = bytes(message).decode('ascii')
-      print(message_ascii, end='')
-      if "ODrive Firmware" in message_ascii:
-        ready = True
-    except:
-      pass
+  
 
 if __name__ == "__main__":
    main(args)