diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index 2d25b89a..697bfec2 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -11,6 +11,9 @@
                 "kind": "build",
                 "isDefault": true
             },
+            "presentation": {
+                "panel": "new"
+            },
             "problemMatcher": [
                 "$gcc"
             ]
diff --git a/Makefile b/Makefile
index ef7d621d..763a496b 100644
--- a/Makefile
+++ b/Makefile
@@ -78,7 +78,8 @@ C_SOURCES = \
   MotorControl/utils.c \
   MotorControl/low_level.c
 CPP_SOURCES = \
-  MotorControl/axis.cpp
+  MotorControl/axis.cpp \
+  MotorControl/protocol.cpp
 ASM_SOURCES = \
   startup/startup_stm32f405xx.s
 
diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c
index 1af2d3df..4e716925 100755
--- a/MotorControl/low_level.c
+++ b/MotorControl/low_level.c
@@ -35,7 +35,7 @@ float vbus_voltage = 12.0f;
 // TODO stick parameter into struct
 #define ENCODER_CPR (600*4)
 #define POLE_PAIRS 7
-static float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR);
+const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR);
 
 // TODO: Migrate to C++, clearly we are actually doing object oriented code here...
 // TODO: For nice encapsulation, consider not having the motor objects public
@@ -208,7 +208,7 @@ Motor_t motors[] = {
         .timing_log = {0}
     }
 };
-const int num_motors = sizeof(motors)/sizeof(motors[0]);
+const size_t num_motors = sizeof(motors) / sizeof(motors[0]);
 
 /* Private constant data -----------------------------------------------------*/
 static const float one_by_sqrt3 = 0.57735026919f;
@@ -219,8 +219,6 @@ static const int current_meas_hz = CURRENT_MEAS_HZ;
 /* Private variables ---------------------------------------------------------*/
 static float brake_resistance = 0.47f; // [ohm]
 
-/* Monitoring */
-monitoring_slot monitoring_slots[20] = {0};
 
 /* variables exposed to usb interface via set/get/monitor
  * If you change something here, don't forget to regenerate the python interface with generate_api.py
@@ -228,8 +226,6 @@ monitoring_slot monitoring_slots[20] = {0};
  * */
 
 float* exposed_floats[] = {
-    &vbus_voltage, // ro
-    &elec_rad_per_enc, // ro
     &motors[0].pos_setpoint, // rw
     &motors[0].pos_gain, // rw
     &motors[0].vel_setpoint, // rw
@@ -323,31 +319,8 @@ uint16_t* exposed_uint16[] = {
 
 //--------------------------------
 // Command Handling
-// TODO move to different file
 //--------------------------------
 
-void print_monitoring(int limit) {
-    for (int i=0;i<limit;i++) {
-        switch (monitoring_slots[i].type) {
-        case 0:
-            printf("%f\t",*exposed_floats[monitoring_slots[i].index]);
-            break;
-        case 1:
-            printf("%d\t",*exposed_ints[monitoring_slots[i].index]);
-            break;
-        case 2:
-            printf("%d\t",*exposed_bools[monitoring_slots[i].index]);
-            break;
-        case 3:
-            printf("%hu\t",*exposed_uint16[monitoring_slots[i].index]);
-            break;
-        default:
-            i=100;
-        }
-    }
-    printf("\n");
-}
-
 void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
     motor->pos_setpoint = pos_setpoint;
     motor->vel_setpoint = vel_feed_forward;
@@ -375,108 +348,6 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint) {
 #endif
 }
 
-void motor_parse_cmd(uint8_t* buffer, int len) {
-
-    // TODO very hacky way of terminating sscanf at end of buffer:
-    // We should do some proper struct packing instead of using sscanf altogether
-    buffer[len] = 0;
-
-    // check incoming packet type
-    if (buffer[0] == 'p') {
-        // position control
-        unsigned motor_number;
-        float pos_setpoint, vel_feed_forward, current_feed_forward;
-        int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
-        if (numscan == 4 && motor_number < num_motors) {
-            set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
-        }
-    } else if (buffer[0] == 'v') {
-        // velocity control
-        unsigned motor_number;
-        float vel_feed_forward, current_feed_forward;
-        int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
-        if (numscan == 3 && motor_number < num_motors) {
-            set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
-        }
-    } else if (buffer[0] == 'c') {
-        // current control
-        unsigned motor_number;
-        float current_feed_forward;
-        int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
-        if (numscan == 2 && motor_number < num_motors) {
-            set_current_setpoint(&motors[motor_number], current_feed_forward);
-        }
-    } else if (buffer[0] == 'g') { // GET
-        // g <0:float,1:int,2:bool,3:uint16> index
-        int type = 0;
-        int index = 0;
-        int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index);
-        if (numscan == 2) {
-            switch(type){
-            case 0: {
-                printf("%f\n",*exposed_floats[index]);
-                break;
-            };
-            case 1: {
-                printf("%d\n",*exposed_ints[index]);
-                break;
-            };
-            case 2: {
-                printf("%d\n",*exposed_bools[index]);
-                break;
-            };
-            case 3: {
-                printf("%hu\n",*exposed_uint16[index]);
-                break;
-            };
-            }
-        }
-    } else if (buffer[0] == 's') { // SET
-        // s <0:float,1:int,2:bool,3:uint16> index value
-        int type = 0;
-        int index = 0;
-        int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index);
-        if (numscan == 2) {
-            switch(type) {
-            case 0: {
-                sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]);
-                break;
-            };
-            case 1: {
-                sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]);
-                break;
-            };
-            case 2: {
-                int btmp = 0;
-                sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp);
-                *exposed_bools[index] = btmp ? true : false;
-                break;
-            };
-            case 3: {
-                sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]);
-                break;
-            };
-            }
-        }
-    } else if (buffer[0] == 'm') { // Setup Monitor
-        // m <0:float,1:int,2:bool,3:uint16> index monitoring_slot
-        int type = 0;
-        int index = 0;
-        int slot = 0;
-        int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot);
-        if (numscan == 3) {
-            monitoring_slots[slot].type = type;
-            monitoring_slots[slot].index = index;
-        }
-    } else if (buffer[0] == 'o') { // Output Monitor
-        int limit = 0;
-        int numscan = sscanf((const char*)buffer, "o %u", &limit);
-        if (numscan == 1) {
-            print_monitoring(limit);
-        }
-    }
-}
-
 
 //--------------------------------
 // Utility
@@ -793,7 +664,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
         // and their interrupts should arrive on the same clock cycle.
         // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
         // Therefore we store the value from ADC2 and signal the thread that the
-        // measurement is ready when we recieve the ADC3 measurement
+        // measurement is ready when we receive the ADC3 measurement
 
         // return or continue
         if (hadc == &hadc2) {
diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h
index 6733522d..f05d1bd4 100644
--- a/MotorControl/low_level.h
+++ b/MotorControl/low_level.h
@@ -2,6 +2,10 @@
 #ifndef __LOW_LEVEL_H
 #define __LOW_LEVEL_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /* Includes ------------------------------------------------------------------*/
 #include <cmsis_os.h>
 #include "drv8301.h"
@@ -150,7 +154,8 @@ typedef struct{
 /* Exported constants --------------------------------------------------------*/
 extern float vbus_voltage;
 extern Motor_t motors[];
-extern const int num_motors;
+extern const size_t num_motors;
+extern const float elec_rad_per_enc;
 
 /* Exported variables --------------------------------------------------------*/
 // Exposed comms table during refactor transition
@@ -213,4 +218,8 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des);
 void control_motor_loop(Motor_t* motor);
 // Motor thread (is public)
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif //__LOW_LEVEL_H
diff --git a/MotorControl/protocol.cpp b/MotorControl/protocol.cpp
new file mode 100644
index 00000000..cf0c3bb6
--- /dev/null
+++ b/MotorControl/protocol.cpp
@@ -0,0 +1,113 @@
+
+#include "low_level.h"
+#include "protocol.h"
+
+#include <memory>
+
+Endpoint endpoints[] = {
+    Endpoint("vbus_voltage", static_cast<const float>(vbus_voltage)),
+    Endpoint("elec_rad_per_enc", 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
+};
+
+constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]);
+
+/* Monitoring */
+size_t monitoring_slots[20] = {0};
+constexpr size_t NUM_MONITORING_SLOTS = sizeof(monitoring_slots) / sizeof(monitoring_slots[0]);
+
+
+void Protocol_print_json(void) {
+    bool need_comma = false;
+    printf("[");
+    for (size_t i = 0; i < NUM_ENDPOINTS; ++i) {
+        endpoints[i].print_json(i, need_comma);
+    }
+    printf("]");
+}
+
+void Protocol_print_monitoring(size_t limit) {
+    for (size_t i = 0; i < limit; ++i) {
+        if (monitoring_slots[i] < NUM_ENDPOINTS) {
+            endpoints[monitoring_slots[i]].print_value();
+        }
+        printf("\t");
+    }
+    printf("\n");
+}
+
+void Protocol_parse_cmd(uint8_t* buffer, int len) {
+
+    // TODO very hacky way of terminating sscanf at end of buffer:
+    // We should do some proper struct packing instead of using sscanf altogether
+    buffer[len] = 0;
+
+    // check incoming packet type
+    if (buffer[0] == 'p') {
+        // position control
+        unsigned motor_number;
+        float pos_setpoint, vel_feed_forward, current_feed_forward;
+        int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
+        if (numscan == 4 && motor_number < num_motors) {
+            set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
+        }
+    } else if (buffer[0] == 'v') {
+        // velocity control
+        unsigned motor_number;
+        float vel_feed_forward, current_feed_forward;
+        int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
+        if (numscan == 3 && motor_number < num_motors) {
+            set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
+        }
+    } else if (buffer[0] == 'c') {
+        // current control
+        unsigned motor_number;
+        float current_feed_forward;
+        int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
+        if (numscan == 2 && motor_number < num_motors) {
+            set_current_setpoint(&motors[motor_number], current_feed_forward);
+        }
+    } else if (buffer[0] == 'j') {
+        // Read JSON interface definition
+        Protocol_print_json();
+    } else if (buffer[0] == 'w') { // WRITE
+        // s index value
+        size_t index = 0;
+        size_t pos = 0;
+        int numscan = sscanf((const char*)buffer, "s %u %n", &index, &pos);
+        if (numscan == 1) {
+            if (index < NUM_ENDPOINTS) {
+                endpoints[index].scan_value((const char*)buffer + pos);
+            }
+        }
+    } else if (buffer[0] == 'r') { // READ
+        // r index
+        size_t index = 0;
+        int numscan = sscanf((const char*)buffer, "r %u", &index);
+        if (numscan == 1) {
+            if (index < NUM_ENDPOINTS) {
+                endpoints[index].print_value();
+            }
+        }
+    } else if (buffer[0] == 'm') { // Setup Monitor
+        // m index monitoring_slot
+        size_t index = 0;
+        size_t slot = 0;
+        int numscan = sscanf((const char*)buffer, "m %u %u", &index, &slot);
+        if (numscan == 2) {
+            if (index < NUM_ENDPOINTS && slot < NUM_MONITORING_SLOTS) {
+                monitoring_slots[slot] = index;
+            }
+        }
+    } else if (buffer[0] == 'o') { // Output Monitor
+        size_t limit = 0;
+        int numscan = sscanf((const char*)buffer, "o %u", &limit);
+        if (numscan == 1) {
+            Protocol_print_monitoring(limit);
+        }
+    }
+}
diff --git a/MotorControl/protocol.h b/MotorControl/protocol.h
new file mode 100644
index 00000000..fa538908
--- /dev/null
+++ b/MotorControl/protocol.h
@@ -0,0 +1,134 @@
+
+// TODO: resolve assert
+#define assert(expr)
+
+
+#ifdef __cplusplus
+
+#include <functional>
+
+typedef std::function<void(const void *ctx)> PrintCallback;
+typedef std::function<void(const char* buffer, void *ctx)> ScanCallback;
+
+typedef enum {
+    AS_FLOAT,
+    AS_INT,
+    AS_BOOL,
+    AS_UINT16,
+    BEGIN_TREE,
+    END_TREE
+} TypeInfo_t;
+
+// The order in this list must correspond to the order in TypeInfo_t
+const char *_type_names[] = {
+    "float",
+    "int",
+    "bool",
+    "uint16",
+    "tree"
+};
+
+
+// Default getters/setters
+
+PrintCallback print_float = std::bind(printf, "%f", std::placeholders::_1);
+ScanCallback scan_float = std::bind(sscanf, std::placeholders::_1, "%f", std::placeholders::_2);
+
+PrintCallback print_int = std::bind(printf, "%d", std::placeholders::_1);
+ScanCallback scan_int = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2);
+
+PrintCallback print_bool = std::bind(printf, "%d", std::placeholders::_1);
+ScanCallback scan_bool = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2);
+
+PrintCallback print_uint16 = std::bind(printf, "%d", std::placeholders::_1);
+ScanCallback scan_uint16 = std::bind(sscanf, std::placeholders::_1, "%d", std::placeholders::_2);
+
+class Endpoint {
+private:
+    const TypeInfo_t _type_info;
+    const PrintCallback _print_callback;
+    ScanCallback _scan_callback;
+    const void* const _ctx;
+
+public:
+    const char* const _name;
+
+    Endpoint(const char* name, TypeInfo_t type_info, PrintCallback print_callback, const void *ctx) :
+        _type_info(type_info),
+        _print_callback(print_callback),
+        _scan_callback(nullptr),
+        _ctx(ctx),
+        _name(name)
+    {
+    }
+
+    Endpoint(const char* name, TypeInfo_t type_info, PrintCallback print_callback, ScanCallback scan_callback, void *ctx) :
+        _type_info(type_info),
+        _print_callback(print_callback),
+        _scan_callback(scan_callback),
+        _ctx(ctx),
+        _name(name)
+    {
+    }
+
+    Endpoint(const char* name, const float& ctx) :
+        Endpoint(name, AS_FLOAT, print_float, &ctx) {}
+    Endpoint(const char* name, float& ctx) :
+        Endpoint(name, AS_FLOAT, print_float, scan_float, &ctx) {}
+
+    Endpoint(const char* name, const int& ctx) :
+        Endpoint(name, AS_INT, print_int, &ctx) {}
+    Endpoint(const char* name, int& ctx) :
+        Endpoint(name, AS_INT, print_int, scan_int, &ctx) {}
+
+    Endpoint(const char* name, const bool& ctx) :
+        Endpoint(name, AS_BOOL, print_bool, &ctx) {}
+    Endpoint(const char* name, bool& ctx) :
+        Endpoint(name, AS_BOOL, print_bool, scan_bool, &ctx) {}
+
+    Endpoint(const char* name, const uint16_t& ctx) :
+        Endpoint(name, AS_UINT16, print_uint16, &ctx) {}
+    Endpoint(const char* name, uint16_t& ctx) :
+        Endpoint(name, AS_UINT16, print_uint16, scan_uint16, &ctx) {}
+
+    void print_json(size_t id, bool& need_comma) {
+        if (_type_info == END_TREE) {
+            printf("]}");
+            need_comma = true;
+            return;
+        } else if (_type_info < END_TREE) {
+            assert(_name);
+            assert(_type_names[_type_info]);
+            if (need_comma)
+                printf(",");
+            printf("{\"name\":\"%s\",\"id\":%u,\"type\":\"%s\"", _name, id, _type_names[_type_info]);
+            if (_type_info == BEGIN_TREE) {
+                printf(",\"content\":[");
+                need_comma = false;
+            } else {
+                printf("}");
+                need_comma = true;
+            }
+        }
+    }
+
+    void print_value(void) {
+        if (_print_callback)
+            _print_callback(_ctx);
+    }
+
+    void scan_value(const char* buffer) {
+        if (_scan_callback)
+            _scan_callback(buffer, const_cast<void*>(_ctx));
+    }
+};
+
+
+extern "C" {
+#endif
+
+void Protocol_parse_cmd(uint8_t* buffer, int len);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/Src/usbd_cdc_if.c b/Src/usbd_cdc_if.c
index 862cd359..ad43a5f8 100644
--- a/Src/usbd_cdc_if.c
+++ b/Src/usbd_cdc_if.c
@@ -50,7 +50,7 @@
 #include "usbd_cdc_if.h"
 /* USER CODE BEGIN INCLUDE */
 #include "utils.h"
-#include "low_level.h"
+#include "protocol.h"
 /* USER CODE END INCLUDE */
 
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
@@ -275,7 +275,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
   int null_idx = MACRO_MIN(*Len, APP_RX_DATA_SIZE-1);
   Buf[null_idx] = 0;
 
-  motor_parse_cmd(Buf, *Len);
+  Protocol_parse_cmd(Buf, *Len);
 
   return (USBD_OK);
   /* USER CODE END 6 */