diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml index 977d5f06..ab77af44 100644 --- a/blue_bringup/package.xml +++ b/blue_bringup/package.xml @@ -11,6 +11,8 @@ Philipp Wu catkin + roscpp + rospy blue_controller_manager blue_msgs diff --git a/blue_hardware_drivers/CMakeLists.txt b/blue_hardware_drivers/CMakeLists.txt index db0affa3..e66a4d47 100644 --- a/blue_hardware_drivers/CMakeLists.txt +++ b/blue_hardware_drivers/CMakeLists.txt @@ -80,7 +80,7 @@ install(TARGETS BLDCDriver_communication_freq_test ## Mark cpp header files for installation install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" + FILES_MATCHING PATTERN "*.h*" ) ## Mark Python scripts for installation @@ -92,3 +92,12 @@ install(PROGRAMS scripts/comms.py scripts/bldc_driver.py scripts/bldc_driver_fre install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) + +# set CMake module path for Cython +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +include(UseCython) +add_custom_target(ReplicatePythonSourceTree ALL ${CMAKE_COMMAND} -P + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/ReplicatePythonSourceTree.cmake + ${CMAKE_CURRENT_BINARY_DIR} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(src/cython) diff --git a/blue_hardware_drivers/cmake/FindCython.cmake b/blue_hardware_drivers/cmake/FindCython.cmake new file mode 100644 index 00000000..7d28eb14 --- /dev/null +++ b/blue_hardware_drivers/cmake/FindCython.cmake @@ -0,0 +1,45 @@ +# Find the Cython compiler. +# +# This code sets the following variables: +# +# CYTHON_EXECUTABLE +# +# See also UseCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Use the Cython executable that lives next to the Python executable +# if it is a local installation. +find_package( PythonInterp ) +if( PYTHONINTERP_FOUND ) + get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat + HINTS ${_python_path} + ) +else() + find_program( CYTHON_EXECUTABLE + NAMES cython cython.bat cython3 + ) +endif() + + +include( FindPackageHandleStandardArgs ) +FIND_PACKAGE_HANDLE_STANDARD_ARGS( Cython REQUIRED_VARS CYTHON_EXECUTABLE ) + +mark_as_advanced( CYTHON_EXECUTABLE ) + diff --git a/blue_hardware_drivers/cmake/ReplicatePythonSourceTree.cmake b/blue_hardware_drivers/cmake/ReplicatePythonSourceTree.cmake new file mode 100644 index 00000000..d308cce7 --- /dev/null +++ b/blue_hardware_drivers/cmake/ReplicatePythonSourceTree.cmake @@ -0,0 +1,4 @@ +# Note: when executed in the build dir, then CMAKE_CURRENT_SOURCE_DIR is the +# build dir. +file( COPY setup.py src test bin DESTINATION "${CMAKE_ARGV3}" + FILES_MATCHING PATTERN "*.py" ) diff --git a/blue_hardware_drivers/cmake/UseCython.cmake b/blue_hardware_drivers/cmake/UseCython.cmake new file mode 100644 index 00000000..0eb13c50 --- /dev/null +++ b/blue_hardware_drivers/cmake/UseCython.cmake @@ -0,0 +1,287 @@ +# Define a function to create Cython modules. +# +# For more information on the Cython project, see http://cython.org/. +# "Cython is a language that makes writing C extensions for the Python language +# as easy as Python itself." +# +# This file defines a CMake function to build a Cython Python module. +# To use it, first include this file. +# +# include( UseCython ) +# +# Then call cython_add_module to create a module. +# +# cython_add_module( ... ) +# +# To create a standalone executable, the function +# +# cython_add_standalone_executable( [MAIN_MODULE src1] ... ) +# +# To avoid dependence on Python, set the PYTHON_LIBRARY cache variable to point +# to a static library. If a MAIN_MODULE source is specified, +# the "if __name__ == '__main__':" from that module is used as the C main() method +# for the executable. If MAIN_MODULE, the source with the same basename as +# is assumed to be the MAIN_MODULE. +# +# Where is the name of the resulting Python module and +# ... are source files to be compiled into the module, e.g. *.pyx, +# *.py, *.c, *.cxx, etc. A CMake target is created with name . This can +# be used for target_link_libraries(), etc. +# +# The sample paths set with the CMake include_directories() command will be used +# for include directories to search for *.pxd when running the Cython complire. +# +# Cache variables that effect the behavior include: +# +# CYTHON_ANNOTATE +# CYTHON_NO_DOCSTRINGS +# CYTHON_FLAGS +# +# Source file properties that effect the build process are +# +# CYTHON_IS_CXX +# +# If this is set of a *.pyx file with CMake set_source_files_properties() +# command, the file will be compiled as a C++ file. +# +# See also FindCython.cmake + +#============================================================================= +# Copyright 2011 Kitware, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +#============================================================================= + +# Configuration options. +set( CYTHON_ANNOTATE OFF + CACHE BOOL "Create an annotated .html file when compiling *.pyx." ) +set( CYTHON_NO_DOCSTRINGS OFF + CACHE BOOL "Strip docstrings from the compiled module." ) +set( CYTHON_FLAGS "" CACHE STRING + "Extra flags to the cython compiler." ) +mark_as_advanced( CYTHON_ANNOTATE CYTHON_NO_DOCSTRINGS CYTHON_FLAGS ) + +find_package( Cython REQUIRED ) +find_package( PythonLibs REQUIRED ) + +set( CYTHON_CXX_EXTENSION "cxx" ) +set( CYTHON_C_EXTENSION "c" ) + +# Create a *.c or *.cxx file from a *.pyx file. +# Input the generated file basename. The generate file will put into the variable +# placed in the "generated_file" argument. Finally all the *.py and *.pyx files. +function( compile_pyx _name generated_file ) + # Default to assuming all files are C. + set( cxx_arg "--cplus" ) + set( extension ${CYTHON_CXX_EXTENSION} ) + set( pyx_lang "CXX" ) + set( comment "Compiling Cython C source for ${_name}..." ) + + set( cython_include_directories "" ) + set( pxd_dependencies "" ) + set( c_header_dependencies "" ) + set( pyx_locations "" ) + + foreach( pyx_file ${ARGN} ) + get_filename_component( pyx_file_basename "${pyx_file}" NAME_WE ) + + # Determine if it is a C or C++ file. + get_source_file_property( property_is_cxx ${pyx_file} CYTHON_IS_CXX ) + if( ${property_is_cxx} ) + set( cxx_arg "--cplus" ) + set( extension ${CYTHON_CXX_EXTENSION} ) + set( pyx_lang "CXX" ) + set( comment "Compiling Cython CXX source for ${_name}..." ) + endif() + + # Get the include directories. + get_source_file_property( pyx_location ${pyx_file} LOCATION ) + get_filename_component( pyx_path ${pyx_location} PATH ) + get_directory_property( cmake_include_directories DIRECTORY ${pyx_path} INCLUDE_DIRECTORIES ) + list( APPEND cython_include_directories ${cmake_include_directories} ) + list( APPEND pyx_locations "${pyx_location}" ) + + # Determine dependencies. + # Add the pxd file will the same name as the given pyx file. + unset( corresponding_pxd_file CACHE ) + find_file( corresponding_pxd_file ${pyx_file_basename}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} + NO_DEFAULT_PATH ) + if( corresponding_pxd_file ) + list( APPEND pxd_dependencies "${corresponding_pxd_file}" ) + endif() + + # pxd files to check for additional dependencies. + set( pxds_to_check "${pyx_file}" "${pxd_dependencies}" ) + set( pxds_checked "" ) + set( number_pxds_to_check 1 ) + while( ${number_pxds_to_check} GREATER 0 ) + foreach( pxd ${pxds_to_check} ) + list( APPEND pxds_checked "${pxd}" ) + list( REMOVE_ITEM pxds_to_check "${pxd}" ) + + # check for C header dependencies + file( STRINGS "${pxd}" extern_from_statements + REGEX "cdef[ ]+extern[ ]+from.*$" ) + foreach( statement ${extern_from_statements} ) + # Had trouble getting the quote in the regex + string( REGEX REPLACE "cdef[ ]+extern[ ]+from[ ]+[\"]([^\"]+)[\"].*" "\\1" header "${statement}" ) + unset( header_location CACHE ) + find_file( header_location ${header} PATHS ${cmake_include_directories} ) + if( header_location ) + list( FIND c_header_dependencies "${header_location}" header_idx ) + if( ${header_idx} LESS 0 ) + list( APPEND c_header_dependencies "${header_location}" ) + endif() + endif() + endforeach() + + # check for pxd dependencies + + # Look for cimport statements. + set( module_dependencies "" ) + file( STRINGS "${pxd}" cimport_statements REGEX cimport ) + foreach( statement ${cimport_statements} ) + if( ${statement} MATCHES from ) + string( REGEX REPLACE "from[ ]+([^ ]+).*" "\\1" module "${statement}" ) + else() + string( REGEX REPLACE "cimport[ ]+([^ ]+).*" "\\1" module "${statement}" ) + endif() + list( APPEND module_dependencies ${module} ) + endforeach() + list( REMOVE_DUPLICATES module_dependencies ) + # Add the module to the files to check, if appropriate. + foreach( module ${module_dependencies} ) + unset( pxd_location CACHE ) + find_file( pxd_location ${module}.pxd + PATHS "${pyx_path}" ${cmake_include_directories} NO_DEFAULT_PATH ) + if( pxd_location ) + list( FIND pxds_checked ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( FIND pxds_to_check ${pxd_location} pxd_idx ) + if( ${pxd_idx} LESS 0 ) + list( APPEND pxds_to_check ${pxd_location} ) + list( APPEND pxd_dependencies ${pxd_location} ) + endif() # if it is not already going to be checked + endif() # if it has not already been checked + endif() # if pxd file can be found + endforeach() # for each module dependency discovered + endforeach() # for each pxd file to check + list( LENGTH pxds_to_check number_pxds_to_check ) + endwhile() + endforeach() # pyx_file + + # Set additional flags. + if( CYTHON_ANNOTATE ) + set( annotate_arg "--annotate" ) + endif() + + if( CYTHON_NO_DOCSTRINGS ) + set( no_docstrings_arg "--no-docstrings" ) + endif() + + if( "${CMAKE_BUILD_TYPE}" STREQUAL "Debug" OR + "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo" ) + set( cython_debug_arg "--gdb" ) + endif() + + # Include directory arguments. + list( REMOVE_DUPLICATES cython_include_directories ) + set( include_directory_arg "" ) + foreach( _include_dir ${cython_include_directories} ) + set( include_directory_arg ${include_directory_arg} "-I" "${_include_dir}" ) + endforeach() + + # Determining generated file name. + set( _generated_file "${CMAKE_CURRENT_BINARY_DIR}/${_name}.${extension}" ) + set_source_files_properties( ${_generated_file} PROPERTIES GENERATED TRUE ) + set( ${generated_file} ${_generated_file} PARENT_SCOPE ) + + list( REMOVE_DUPLICATES pxd_dependencies ) + list( REMOVE_DUPLICATES c_header_dependencies ) + + # Add the command to run the compiler. + add_custom_command( OUTPUT ${_generated_file} + COMMAND ${CYTHON_EXECUTABLE} + ARGS ${cxx_arg} ${include_directory_arg} + ${annotate_arg} ${no_docstrings_arg} ${cython_debug_arg} ${CYTHON_FLAGS} + --output-file ${_generated_file} ${pyx_locations} + DEPENDS ${pyx_locations} ${pxd_dependencies} + IMPLICIT_DEPENDS ${pyx_lang} ${c_header_dependencies} + COMMENT ${comment} + ) + + # Remove their visibility to the user. + set( corresponding_pxd_file "" CACHE INTERNAL "" ) + set( header_location "" CACHE INTERNAL "" ) + set( pxd_location "" CACHE INTERNAL "" ) +endfunction() + +# cython_add_module( src1 src2 ... srcN ) +# Build the Cython Python module. +function( cython_add_module _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + foreach( _file ${ARGN} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + list( APPEND pyx_module_sources ${_file} ) + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + compile_pyx( ${_name} generated_file ${pyx_module_sources} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + python_add_module( ${_name} ${generated_file} ${other_module_sources} ) + if( APPLE ) + set_target_properties( ${_name} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup" ) + else() + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ) + endif() +endfunction() + +include( CMakeParseArguments ) +# cython_add_standalone_executable( _name [MAIN_MODULE src3.py] src1 src2 ... srcN ) +# Creates a standalone executable the given sources. +function( cython_add_standalone_executable _name ) + set( pyx_module_sources "" ) + set( other_module_sources "" ) + set( main_module "" ) + cmake_parse_arguments( cython_arguments "" "MAIN_MODULE" "" ${ARGN} ) + include_directories( ${PYTHON_INCLUDE_DIRS} ) + foreach( _file ${cython_arguments_UNPARSED_ARGUMENTS} ) + if( ${_file} MATCHES ".*\\.py[x]?$" ) + get_filename_component( _file_we ${_file} NAME_WE ) + if( "${_file_we}" STREQUAL "${_name}" ) + set( main_module "${_file}" ) + elseif( NOT "${_file}" STREQUAL "${cython_arguments_MAIN_MODULE}" ) + set( PYTHON_MODULE_${_file_we}_static_BUILD_SHARED OFF ) + compile_pyx( "${_file_we}_static" generated_file "${_file}" ) + list( APPEND pyx_module_sources "${generated_file}" ) + endif() + else() + list( APPEND other_module_sources ${_file} ) + endif() + endforeach() + + if( cython_arguments_MAIN_MODULE ) + set( main_module ${cython_arguments_MAIN_MODULE} ) + endif() + if( NOT main_module ) + message( FATAL_ERROR "main module not found." ) + endif() + get_filename_component( main_module_we "${main_module}" NAME_WE ) + set( CYTHON_FLAGS ${CYTHON_FLAGS} --embed ) + compile_pyx( "${main_module_we}_static" generated_file ${main_module} ) + add_executable( ${_name} ${generated_file} ${pyx_module_sources} ${other_module_sources} ) + target_link_libraries( ${_name} ${PYTHON_LIBRARIES} ${pyx_module_libs} ) +endfunction() diff --git a/blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.h b/blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.hpp similarity index 100% rename from blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.h rename to blue_hardware_drivers/include/blue_hardware_drivers/BLDCControllerClient.hpp diff --git a/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h b/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h index 3d8d69e3..1726d801 100644 --- a/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h +++ b/blue_hardware_drivers/include/blue_hardware_drivers/BLDCDriver.h @@ -14,7 +14,7 @@ #include "time.h" #include "blue_msgs/MotorState.h" -#include "blue_hardware_drivers/BLDCControllerClient.h" +#include "blue_hardware_drivers/BLDCControllerClient.hpp" namespace blue_hardware_drivers { diff --git a/blue_hardware_drivers/package.xml b/blue_hardware_drivers/package.xml index 1ebbf9d2..418ad252 100644 --- a/blue_hardware_drivers/package.xml +++ b/blue_hardware_drivers/package.xml @@ -17,6 +17,7 @@ roscpp rospy + cython sensor_msgs serial std_msgs diff --git a/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py b/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py index 5730d32f..58d0569f 100755 --- a/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py +++ b/blue_hardware_drivers/scripts/bldc_driver_frequency_test.py @@ -1,15 +1,7 @@ #!/usr/bin/env python -from comms import BLDCControllerClient -import time -import serial -import math -import signal +from blue_hardware_drivers import PyBLDCControllerClient import sys import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Float64 -from std_msgs.msg import Float32 -from comms import * # Example usage: # rosrun blue_hardware_drivers bldc_driver_frequency_test.py 2,42,41,33,16,31,32,14 /dev/ttyUSB1 @@ -24,9 +16,7 @@ def main(): rospy.init_node('freq_publisher', anonymous=True) - motor_ids = [int(x) for x in sys.argv[1].split(",")] - if len(sys.argv) >= 3: port = sys.argv[2] else: @@ -34,73 +24,12 @@ def main(): rospy.loginfo("Testing communication frequency: {} {}".format(motor_ids, port)) - s = serial.Serial(port=port, baudrate=1000000, timeout=0.01) - device = BLDCControllerClient(s) - time.sleep(0.1) - - device.leaveBootloader(motor_ids) - - time.sleep(0.2) - s.flush() - time.sleep(0.1) - - - freq_pub = rospy.Publisher("/freq", Float32, queue_size=1) - msg = Float32() - - last_time = rospy.get_time() - - for id in motor_ids: - success = False - for attempt in range(5): - try: - rospy.loginfo("Calibrating motor %d..." % id) - #calibrations = device.readCalibration([id]) - #print(calibrations) - #device.setZeroAngle([id], [calibrations['angle']]) - #device.setInvertPhases([id], [calibrations['inv']]) - #device.setERevsPerMRev([id], [calibrations['epm']]) - #device.setTorqueConstant([id], [calibrations['torque']]) - #device.setPositionOffset([id], [calibrations['zero']]) - device._ser.read_all() - device.setZeroAngle([id], [1169]) - device.setInvertPhases([id], [1]) - device.setERevsPerMRev([id], [14]) - device.setTorqueConstant([id], [1.45]) - device.setPositionOffset([id], [0.0]) - device.setCurrentControlMode([id]) - #starting_angles[id] = 0.0 - device.writeRegisters([id], [0x1030], [1], [struct.pack(' #include #include diff --git a/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp b/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp index 72f8cf0f..10d87f1d 100644 --- a/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp +++ b/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp @@ -4,7 +4,7 @@ #include "std_msgs/Float64.h" #include "std_msgs/Float32.h" #include "sensor_msgs/JointState.h" -#include "blue_hardware_drivers/BLDCControllerClient.h" +#include "blue_hardware_drivers/BLDCControllerClient.hpp" #include #include #include "math.h" diff --git a/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx new file mode 100644 index 00000000..e708a137 --- /dev/null +++ b/blue_hardware_drivers/src/cython/BLDCControllerClient.pyx @@ -0,0 +1,119 @@ +# distutils: language = c++ +# cython: c_string_type=unicode, c_string_encoding=utf8 + +from libcpp cimport bool +import cython +from libc.stdlib cimport malloc, free +from libcpp.string cimport string +from libcpp.vector cimport vector + +ctypedef unsigned char comm_id_t + +cdef extern from "blue_hardware_drivers/BLDCControllerClient.hpp" namespace "blue_hardware_drivers": + cppclass BLDCControllerClient: + BLDCControllerClient(string port, vector[comm_id_t] boards) except + + + # # Initialize Board ID for Disco Bus Protocol + queueConfirmID(comm_id_t board_id); + queueEnumerate(comm_id_t board_id); + getEnumerateResponse(comm_id_t board_id, int* response_id); + + # # Program Counter Adjustments + # queueLeaveBootloader(comm_id_t board_id, uint32_t jump_addr); + + # Calibration Setup + # queueSetTimeout(comm_id_t board_id, uint16_t value); + # queueSetControlMode(comm_id_t board_id, comm_ctrl_mode_t control_mode); + # queueSetZeroAngle(comm_id_t board_id, uint16_t value); + # queueSetERevsPerMRev(comm_id_t board_id, uint8_t value); + # queueSetInvertPhases(comm_id_t board_id, uint8_t value); + # queueSetTorqueConstant(comm_id_t board_id, float value); + # queueSetPositionOffset(comm_id_t board_id, float value); + # queueSetEACScale(comm_id_t board_id, float value); + # queueSetEACOffset(comm_id_t board_id, float value); + # queueSetEACTable(comm_id_t board_id, size_t start_index, uint8_t *values, size_t count); + queueSetDirectCurrentControllerKp(comm_id_t board_id, float value); + queueSetDirectCurrentControllerKi(comm_id_t board_id, float value); + queueSetQuadratureCurrentControllerKp(comm_id_t board_id, float value); + queueSetQuadratureCurrentControllerKi(comm_id_t board_id, float value); + queueSetVelocityControllerKp(comm_id_t board_id, float value); + queueSetVelocityControllerKi(comm_id_t board_id, float value); + queueSetPositionControllerKp(comm_id_t board_id, float value); + queueSetPositionControllerKi(comm_id_t board_id, float value); + # queueSetIAOffset(comm_id_t board_id, float value); + # queueSetIBOffset(comm_id_t board_id, float value); + # queueSetICOffset(comm_id_t board_id, float value); + + # # Drive Commands + queueSetCommand(comm_id_t board_id, float value); + queueSetPosCommand(comm_id_t board_id, float position, float feedforward); + queueGetRotorPosition(comm_id_t board_id); + queueSetCommandAndGetRotorPosition(comm_id_t board_id, float value); + queueSetPositionAndGetRotorPosition(comm_id_t board_id, float value); + queueGetState(comm_id_t board_id); + queueSetCommandAndGetState(comm_id_t board_id, float value); + queueSetPosCommandAndGetState(comm_id_t board_id, float position, float feedforward); + + # Init Motor State Commands + # queueSetRevolutions(comm_id_t board_id, int16_t value); + + # Send queued packets and receive from boards + exchange(); + + # Remove all items currently in the queue + clearQueue(); + + # Send all boards to bootloader + resetBoards(); + + # Clear the RS485 Buffer + resetBuffer(); + + # # Check if a watchdog reset has occurred on a given board + bool checkWDGRST(comm_id_t board_id); + bool checkTimeout(comm_id_t board_id); + queueClearWDGRST(comm_id_t board_id); + + # # Result Commands + resultGetRotorPosition(comm_id_t board_id, float* result); + # resultGetState(comm_id_t board_id, float* position, float* velocity, float* di, float* qi, float* voltage, float* temp, int32_t* acc_x, int32_t* acc_y, int32_t* acc_z); + + # # Setup/Programming Commands + bool initMotor(comm_id_t board_id); + +cdef class PyBLDCControllerClient: + cdef BLDCControllerClient*c_bldc_client + + def __cinit__( + PyBLDCControllerClient self, + string c_port, + boards + ): + cdef vector[comm_id_t] c_boards + for i in range(len(boards)): + c_boards.push_back(i) + self.c_bldc_client = new BLDCControllerClient(c_port, c_boards) + + def queue_get_rotor_position(self, comm_id_t board_id): + self._bldc_client.queueGetRotorPosition(board_id) + + def __dealloc__(self): + del self.c_bldc_client + + def exchange(self): + self._bldc_client.exchange() + + def clear_queue(self): + self._bldc_client.clearQueue() + + def reset_boards(self): + self._bldc_client.resetBoards() + + def reset_buffer(self): + self._bldc_client.resetBuffer() + + def init_motor(self, board_id): + self._bldc_client.initMotor(board_id) + + def result_get_rotor_position(self, board_id, result): + self._bldc_client.resultGetRotorPosition(board_id, result) diff --git a/blue_hardware_drivers/src/cython/CMakeLists.txt b/blue_hardware_drivers/src/cython/CMakeLists.txt new file mode 100644 index 00000000..589ab8a0 --- /dev/null +++ b/blue_hardware_drivers/src/cython/CMakeLists.txt @@ -0,0 +1,18 @@ +# CAUTION: +# - You can specify only one .pyx file for each cython module. +# - The name of the module and the .pyx file should be identical. +cython_add_module(BLDCControllerClient BLDCControllerClient.pyx ../BLDCControllerClient.cpp) + +# The library (libros_cython_example.so) is created in upper CMakeLists.txt +target_link_libraries(BLDCControllerClient blue_hardware_drivers) + +# Set the targets to be copied in the python package directry. +set_target_properties(BLDCControllerClient + PROPERTIES + LIBRARY_OUTPUT_DIRECTORY + ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}) + +#install(TARGETS add_two_ints sub_two_ints +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) +