Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-20 22:55:00 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

make remote precedure calls work

Browse Source
This commit is contained in:
Samuel Sadok
2017-11-06 14:38:15 +01:00
parent ba46e5fb29
commit 3cba81e6f7
7 changed files with 146 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/commands.cpp
+15 -12
View File
@@ -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.
Firmware/MotorControl/protocol.cpp
+1 -1
View File
@@ -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);
Firmware/MotorControl/protocol.hpp
+2 -4
View File
@@ -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() {
tools/demo.py
+3 -1
View File
@@ -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)
tools/odrive/core.py
+87 -38
View File
@@ -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):
tools/odrive/protocol.py
+1 -9
View File
@@ -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):
tools/test_communication.py
+37 -10
View File
@@ -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):