From 3cba81e6f70bdf98f8c04864b2d9fafacdfce5f0 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Mon, 6 Nov 2017 14:38:15 +0100
Subject: [PATCH] make remote precedure calls work

---
 Firmware/MotorControl/commands.cpp |  27 ++++---
 Firmware/MotorControl/protocol.cpp |   2 +-
 Firmware/MotorControl/protocol.hpp |   6 +-
 tools/demo.py                      |   4 +-
 tools/odrive/core.py               | 125 ++++++++++++++++++++---------
 tools/odrive/protocol.py           |  10 +--
 tools/test_communication.py        |  47 ++++++++---
 7 files changed, 146 insertions(+), 75 deletions(-)

diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp
index 436889b6..4864beee 100644
--- a/Firmware/MotorControl/commands.cpp
+++ b/Firmware/MotorControl/commands.cpp
@@ -39,18 +39,21 @@ static const GpioMode_t gpio_mode = GPIO_MODE_UART;     //GPIO 1,2 is UART Tx,Rx
 /* Variables exposed to USB & UART via read/write commands */
 // 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)
-);
+void motors_0_set_pos_setpoint_func(void) {
+    set_pos_setpoint(&motors[0],
+        motors[0].set_pos_setpoint_args.pos_setpoint,
+        motors[0].set_pos_setpoint_args.vel_feed_forward,
+        motors[0].set_pos_setpoint_args.current_feed_forward);
+}
+void motors_0_set_vel_setpoint_func(void) {
+    set_vel_setpoint(&motors[0],
+        motors[0].set_vel_setpoint_args.vel_setpoint,
+        motors[0].set_vel_setpoint_args.current_feed_forward);
+}
+void motors_0_set_current_setpoint_func(void) {
+    set_current_setpoint(&motors[0],
+        motors[0].set_current_setpoint_args.current_setpoint);
+}
 
 // clang-format off
 // TODO: Autogenerate this table. It will come up again very soon in the Arduino library.
diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp
index 41b80ccc..09837449 100644
--- a/Firmware/MotorControl/protocol.cpp
+++ b/Firmware/MotorControl/protocol.cpp
@@ -75,7 +75,7 @@ void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const
         }
 
         if (type_ == BEGIN_OBJECT) {
-            write_string(",\"content\":[", output);
+            write_string(",\"members\":[", output);
             *need_comma = false;
         } else if (type_ == BEGIN_FUNCTION) {
             write_string(",\"arguments\":[", output);
diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp
index 17386219..77a6e664 100644
--- a/Firmware/MotorControl/protocol.hpp
+++ b/Firmware/MotorControl/protocol.hpp
@@ -376,11 +376,9 @@ public:
             "\"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*>();
+    static Endpoint make_function(const char* name, void(*function)(void)) {
         return Endpoint(name, BEGIN_FUNCTION, trigger_endpoint_handler,
-            "\"type\":\"function\"", reinterpret_cast<void*>(f));
+            "\"type\":\"function\"", reinterpret_cast<void*>(function));
     }
 
     static Endpoint close_tree() {
diff --git a/tools/demo.py b/tools/demo.py
index 808d46c6..d8b88615 100755
--- a/tools/demo.py
+++ b/tools/demo.py
@@ -24,13 +24,15 @@ print("Bus voltage is " + str(my_drive.vbus_voltage) + "V")
 my_drive.motor0.pos_setpoint = 3.14
 print("Position setpoint is " + str(my_drive.motor0.pos_setpoint))
 
+# And this is how function calls are done:
+my_drive.motor0.set_pos_setpoint(0.0, 0.0, 0.0)
 
 # little sine wave to test
 t0 = time.monotonic()
 while True:
     setpoint = 10000.0 * math.sin((time.monotonic() - t0)*2)
     print("goto " + str(int(setpoint)))
-    my_drive.motor0.pos_setpoint = setpoint
+    my_drive.motor0.set_pos_setpoint(setpoint, 0.0, 0.0)
     time.sleep(0.01)
 
 
diff --git a/tools/odrive/core.py b/tools/odrive/core.py
index c8787ce4..e04ea476 100644
--- a/tools/odrive/core.py
+++ b/tools/odrive/core.py
@@ -18,6 +18,7 @@ import os
 import odrive.protocol
 import itertools
 import struct
+import functools
 
 def noprint(x):
   pass
@@ -46,57 +47,104 @@ class SimpleDeviceProperty(property):
         self._channel.remote_endpoint_operation(self._id, buffer, True, 0)
 
 
-def create_object(json_data, namespace, channel, printer=noprint):
+
+def call_remote_function(channel, trigger_id, arg_properties, *args):
+    if (len(arg_properties) != len(args)):
+        raise TypeError("expected {} arguments but have {}".format(len(arg_properties), len(args)))
+    for i in range(len(args)):
+        arg_properties[i].fset(None, args[i])
+    channel.remote_endpoint_operation(trigger_id, None, True, 0)
+
+def raise_if_undefined(self, name, value):
+    if hasattr(self, name):
+        object.__setattr__(self, name, value)
+    else:
+        raise TypeError('Cannot set name %r on object of type %s' % (
+                        name, self.__class__.__name__))
+
+def create_property(name, json_data, channel, printer):
+    name = name or "[anonymous]"
+
+    type_str = json_data.get("type", None)
+    if type_str is None:
+        printer("property {} has no specified type".format(name))
+        return None
+
+    if type_str == "float":
+        property_type = float
+        struct_format = "<f"
+    elif type_str == "int":
+        property_type = int
+        struct_format = "<i"
+    elif type_str == "bool":
+        property_type = bool
+        struct_format = "<?"
+    elif type_str == "uint16":
+        property_type = int
+        struct_format = "<H"
+    else:
+        printer("property {} has unsupported type {}".format(name, type_str))
+        return None
+
+    id_str = json_data.get("id", None)
+    if id_str is None:
+        printer("property {} has no specified ID".format(name))
+        return None
+
+    access_mode = json_data.get("mode", "rw")
+    return SimpleDeviceProperty(channel, id_str, property_type,
+                                struct_format,
+                                'r' in access_mode,
+                                'w' in access_mode)
+
+def create_function(name, json_data, channel, printer):
+    id_str = json_data.get("id", None)
+    if id_str is None:
+        printer("function {} has no specified ID".format(name))
+        return None
+
+    inputs = []
+    for param in json_data.get("arguments", []):
+        param["mode"] = "r"
+        inputs.append(create_property(json_data["name"], param, channel, printer))
+    return functools.partial(call_remote_function, channel, id_str, inputs)
+
+def create_object(name, json_data, namespace, channel, printer=noprint):
     """
     Creates an object that implements the specified JSON type description by
     communicating with the provided device object
     """
 
-    # Build property list from JSON
-    properties = {}
-    for item in json_data:
-        name = item.get("name", None)
-        if name is None:
-            printer("unnamed property in {}".format(namespace))
+    if not namespace is None:
+        namespace = namespace + "." + name
+    else:
+        namespace = name
+
+    # Build attribute list from JSON
+    attributes = {"__setattr__": raise_if_undefined}
+    for member in json_data.get("members", []):
+        member_name = member.get("name", None)
+        if member_name is None:
+            printer("ignoring unnamed attribute in {}".format(namespace))
             continue
 
-        type_str = item.get("type", None)
+        type_str = member.get("type", None)
         if type_str is None:
-            printer("property {} has no specified type".format(name))
+            printer("member {} has no specified type".format(member_name))
             continue
 
         if type_str == "object":
-            properties[name] = create_object(item["content"], namespace + "." + item["name"], channel, printer=printer)
+            attribute = create_object(member_name, member, namespace, channel, printer=printer)
+        elif type_str == "function":
+            attribute = create_function(member_name, member, channel, printer)
         else:
-            if type_str == "float":
-                property_type = float
-                struct_format = "<f"
-            elif type_str == "int":
-                property_type = int
-                struct_format = "<i"
-            elif type_str == "bool":
-                property_type = bool
-                struct_format = "<?"
-            elif type_str == "uint16":
-                property_type = int
-                struct_format = "<H"
-            else:
-                printer("property {} has unsupported type {}".format(name or "[anonymous]", type_str))
-                continue
-
-            id_str = item.get("id", None)
-            if id_str is None:
-                printer("property {} has specified ID".format(name))
-                continue
-
-            access_mode = item.get("mode", "rw")
-            properties[name] = SimpleDeviceProperty(channel, id_str, property_type,
-                                            struct_format,
-                                            'r' in access_mode,
-                                            'w' in access_mode)
+            attribute = create_property(member_name, member, channel, printer)
+        
+        if not attribute is None:
+            attributes[member_name] = attribute
 
     # Create a type from the property list and instantiate it
-    jit_type = type(namespace, (object,), properties)
+    jit_type = type(namespace, (object,), attributes)
     new_object = jit_type()
     return new_object
 
@@ -204,7 +252,8 @@ def find_all(printer=noprint):
         except json.decoder.JSONDecodeError:
             printer("device responded on endpoint 0 with something that is not JSON")
             continue
-        yield create_object(json_data, "odrive", channel, printer=printer)
+        json_data = {"name": "odrive", "members": json_data}
+        yield create_object("odrive", json_data, None, channel, printer=printer)
 
 
 def find_any(printer=noprint):
diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py
index d7f239f2..e03bd7f8 100644
--- a/tools/odrive/protocol.py
+++ b/tools/odrive/protocol.py
@@ -80,7 +80,6 @@ class StreamToPacketConverter(StreamSink):
         are received, they are sent to this instance's output PacketSink.
         Incomplete packets are buffered between subsequent calls to this function.
         """
-        result = None
 
         for byte in bytes:
             if (len(self._header) < 3):
@@ -101,18 +100,11 @@ class StreamToPacketConverter(StreamSink):
             # 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):
                 if calc_crc16(CRC16_INIT, self._packet) == 0:
-                    try:
-                        self._output.process_packet(self._packet[:-2])
-                    except Exception as ex:
-                        result = ex
+                    self._output.process_packet(self._packet[:-2])
                 self._header = []
                 self._packet = []
                 self._packet_length = 0
 
-        if isinstance(result, Exception):
-            # TODO: check if this is valid code (pylint complains)
-            raise result
-
 
 class PacketToStreamConverter(PacketSink):
     def __init__(self, output):
diff --git a/tools/test_communication.py b/tools/test_communication.py
index c619a472..f4a69cd5 100755
--- a/tools/test_communication.py
+++ b/tools/test_communication.py
@@ -27,6 +27,19 @@ import odrive.core
 def noprint(str):
   pass
 
+def print_usage():
+  print("ODrive Control Utility")
+  print("---------------------------------------------------------------------")
+  print("USAGE:")
+  print("\tPOSITION_CONTROL:\n\t\tp MOTOR_NUMBER POSITION VELOCITY CURRENT")
+  print("\tVELOCITY_CONTROL:\n\t\tv MOTOR_NUMBER VELOCITY CURRENT")
+  print("\tCURRENT_CONTROL:\n\t\tc MOTOR_NUMBER CURRENT")
+  #print("\tList parameters:\n\t\tmotor0.[TAB]")
+  #print("\tShow parameter:\n\t\tmotor0.pos_setpoint")
+  #print("\tChange parameter:\n\t\tmotor0.pos_setpoint = 0")
+  print("\tQuit Python Script:\n\t\tq")
+  print("---------------------------------------------------------------------")
+
 def command_prompt_loop(device, history):
   """
   Presents the command prompt indefinitely until something goes wrong
@@ -60,7 +73,28 @@ def command_prompt_loop(device, history):
       except (ValueError, IndexError):
         print("invalid command format")
         continue
-      motor.pos_setpoint = pos
+      motor.set_pos_setpoint(pos, vel, cur)
+    elif command.startswith("v "):
+      args = command[2:].split()
+      try:
+        motor = motors[int(args[0])]
+        vel = float(args[1])
+        cur = float(args[2])
+      except (ValueError, IndexError):
+        print("invalid command format")
+        continue
+      motor.set_vel_setpoint(vel, cur)
+    elif command.startswith("c "):
+      args = command[2:].split()
+      try:
+        motor = motors[int(args[0])]
+        cur = float(args[1])
+      except (ValueError, IndexError):
+        print("invalid command format")
+        continue
+      motor.set_current_setpoint(cur)
+    elif command == "h" or command == '?' or command == 'help':
+      print_usage()
     elif command == "q" or command == 'exit':
       sys.exit()
     else:
@@ -72,17 +106,10 @@ def main(args):
   else:
     printer = noprint
 
-  print("ODrive Control Utility")
-  print("---------------------------------------------------------------------")
-  print("USAGE:")
-  print("\tPOSITION_CONTROL:\n\t\tp MOTOR_NUMBER POSITION VELOCITY CURRENT")
-  print("\tVELOCITY_CONTROL:\n\t\tv MOTOR_NUMBER VELOCITY CURRENT")
-  print("\tCURRENT_CONTROL:\n\t\tc MOTOR_NUMBER CURRENT")
-  print("\tQuit Python Script:\n\t\tq")
-  print("---------------------------------------------------------------------")
-
   history = prompt_toolkit.history.InMemoryHistory()
 
+  print_usage()
+
   while True:
     # Connect to device
     if (args.device is None):