diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml new file mode 100644 index 0000000..5fdf2df --- /dev/null +++ b/.github/workflows/industrial_ci.yaml @@ -0,0 +1,16 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: noetic, ROS_REPO: main} + - {ROS_DISTRO: melodic, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index a2d5254..0d3fcb9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -109,7 +109,7 @@ find_package(catkin REQUIRED COMPONENTS ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( - INCLUDE_DIRS include + INCLUDE_DIRS include include/tinymovr LIBRARIES tinymovr_ros CATKIN_DEPENDS controller_interface controller_manager roscpp rospy std_msgs sensor_msgs # DEPENDS system_lib @@ -123,16 +123,32 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( include + include/tinymovr ${catkin_INCLUDE_DIRS} ) +set(TINYMOVR_SOURCES + src/tinymovr/can.cpp + src/tinymovr/comms.cpp + src/tinymovr/controller.cpp + src/tinymovr/current.cpp + src/tinymovr/encoder.cpp + src/tinymovr/motor.cpp + src/tinymovr/position.cpp + src/tinymovr/scheduler.cpp + src/tinymovr/tinymovr.cpp + src/tinymovr/traj_planner.cpp + src/tinymovr/velocity.cpp + src/tinymovr/voltage.cpp + src/tinymovr/watchdog.cpp + ) + ## Declare a C++ library # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/tinymovr_ros.cpp # ) -add_library(${PROJECT_NAME}_hw_iface src/socketcan_cpp.cpp src/tinymovr_can.cpp src/tm_hw_iface.cpp) -add_library(${PROJECT_NAME}_diffbot_hw_iface src/socketcan_cpp.cpp src/tinymovr_can.cpp src/diffbot_hw_iface.cpp) +add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp/socketcan_cpp.cpp src/tm_joint_iface.cpp) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -142,8 +158,7 @@ add_library(${PROJECT_NAME}_diffbot_hw_iface src/socketcan_cpp.cpp src/tinymovr_ ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node src/tm_hw_iface_node.cpp) -add_executable(${PROJECT_NAME}_diffbot_node src/diffbot_hw_iface_node.cpp) +add_executable(tinymovr_joint_iface_node src/tm_joint_iface_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -156,13 +171,8 @@ add_executable(${PROJECT_NAME}_diffbot_node src/diffbot_hw_iface_node.cpp) # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME}_hw_iface - ${catkin_LIBRARIES} -) - -target_link_libraries(${PROJECT_NAME}_diffbot_node - ${PROJECT_NAME}_diffbot_hw_iface +target_link_libraries(tinymovr_joint_iface_node + ${PROJECT_NAME}_joint_iface ${catkin_LIBRARIES} ) diff --git a/README.md b/README.md index 9f735aa..d7181e3 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,76 @@ +__Tinymovr ROS Hardware Interface__ -https://catkin-tools.readthedocs.io/en/latest/installing.html +A ROS package that provides hardware interfacing for Tinymovr devices. This interface allows for seamless integration of Tinymovr devices with ROS-based robotic systems, offering joint state and control through ROS topics and services. -https://www.howtoinstall.me/ubuntu/18-04/liburdfdom-headers-dev/ +## Features +- Real-time reading of joint positions, velocities, and efforts. +- Ability to send joint command setpoints. +- Error handling and robust exception management. +- Compatible with standard ROS controllers, enabling a plug-and-play experience. -https://github.com/siposcsaba89/socketcan-cpp +## Prerequisites -https://stackoverflow.com/questions/17808084/fatal-error-libudev-h-no-such-file-or-directory +- ROS (Robot Operating System) - Tested with ROS Noetic, but should be compatible with other versions. +- SocketCAN tools and utilities installed. +- Tinymovr devices properly set up and calibrated. + +## Installation + +1. Navigate to your catkin workspace's source folder: + +```bash +cd ~/catkin_ws/src/ +``` + +2. Clone the repository: + +```bash +git clone git@github.com:tinymovr/Tinymovr-ROS.git +``` + +3. Build your catkin workspace: + +```bash +cd ~/catkin_ws/ +catkin_make +``` + +4. Source the workspace: + +```bash +source devel/setup.bash +``` + +## Run the Diffbot demo! + +1. Ensure your Tinymovr instances are calibrated and well tuned, test functioning using Tinymovr Studio or CLI. + +2. Configure your hardware in `config/hardware.yaml` and diff drive config in `config/diff_drive_config.yaml` + +3. Start the `tinymovr_diffbot_demo_node` node: + +```bash +roslaunch tinymovr_ros tinymovr_diffbot_demo_node.launch +``` + +4. Spin up a keyboard teleop and drive your robot: + +```bash +rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/diff_drive_controller/cmd_vel +``` + +## Configuration + +To customize the behavior of the Tinymovr ROS, adjust the parameters in the `config` directory. Here, you can set specifics about each joint, including joint names, IDs, and other parameters relevant to your hardware setup. + +## API Documentation + +Further details about the API and individual functions can be found in the generated Doxygen documentation. Please refer to the documentation for advanced use cases. + +## Contributing + +Contributions to improve and expand the functionality of Tinymovr ROS are welcome! Please open an issue or submit a pull request on the GitHub repository. + +## License + +This package is licensed under the [MIT License](LICENSE). diff --git a/avlos_config.yaml b/avlos_config.yaml new file mode 100644 index 0000000..5a53284 --- /dev/null +++ b/avlos_config.yaml @@ -0,0 +1,11 @@ + +# All paths relative to this config file + +generators: + generator_cpp: + enabled: true + paths: + output_helpers: ./include/tinymovr/helpers.hpp + output_header: ./include/tinymovr/tinymovr.hpp + output_impl: ./src/tinymovr/tinymovr.cpp + diff --git a/config/diff_drive_config.yaml b/config/diff_drive_config.yaml new file mode 100644 index 0000000..e722020 --- /dev/null +++ b/config/diff_drive_config.yaml @@ -0,0 +1,10 @@ +diff_drive_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: "wheel_1" + right_wheel: "wheel_2" + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + publish_rate: 50.0 + wheel_separation: 0.4 + wheel_radius: 0.12 + cmd_vel_timeout: 0.5 diff --git a/config/diffbot_hw_iface.yaml b/config/diffbot_hw_iface.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/hardware.yaml b/config/hardware.yaml new file mode 100644 index 0000000..a292dd9 --- /dev/null +++ b/config/hardware.yaml @@ -0,0 +1,19 @@ +tinymovr_joint_iface: + joints: + wheel_2: + # hardware ID of the actuator + id: 2 + offset: 3.14159265359 # in rad + delay_us: 200 + #max-speed: 4.0 # in rad/s + rads_to_ticks: 14.323944878 # how many ticks in a rad + command_interface: velocity + wheel_1: + # hardware ID of the actuator + id: 1 + offset: 3.14159265359 # in rad + delay_us: 200 + #max-speed: 4.0 # in rad/s + rads_to_ticks: 14.323944878 # how many ticks in a rad + command_interface: velocity + diff --git a/config/tm_hw_iface.yaml b/config/tm_hw_iface.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/include/socketcan_cpp/socketcan_cpp.hpp b/include/socketcan_cpp/socketcan_cpp.hpp index a652239..f5f5bf2 100644 --- a/include/socketcan_cpp/socketcan_cpp.hpp +++ b/include/socketcan_cpp/socketcan_cpp.hpp @@ -60,4 +60,4 @@ namespace scpp std::string m_interface; SocketMode m_socket_mode; }; -} +} \ No newline at end of file diff --git a/include/tinymovr/can.hpp b/include/tinymovr/can.hpp new file mode 100644 index 0000000..6f44d56 --- /dev/null +++ b/include/tinymovr/can.hpp @@ -0,0 +1,24 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Can_ : Node +{ + public: + + Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + uint32_t get_rate(void); + void set_rate(uint32_t value); + uint32_t get_id(void); + void set_id(uint32_t value); + +}; diff --git a/include/tinymovr/comms.hpp b/include/tinymovr/comms.hpp new file mode 100644 index 0000000..813d232 --- /dev/null +++ b/include/tinymovr/comms.hpp @@ -0,0 +1,23 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include +#include + +class Comms_ : Node +{ + public: + + Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , can(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + Can_ can; + +}; diff --git a/include/tinymovr/controller.hpp b/include/tinymovr/controller.hpp new file mode 100644 index 0000000..e8b4e09 --- /dev/null +++ b/include/tinymovr/controller.hpp @@ -0,0 +1,44 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include +#include +#include +#include +#include + +class Controller_ : Node +{ + public: + + Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , position(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , velocity(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , current(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , voltage(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + uint8_t get_state(void); + void set_state(uint8_t value); + uint8_t get_mode(void); + void set_mode(uint8_t value); + uint8_t get_warnings(void); + uint8_t get_errors(void); + Position_ position; + Velocity_ velocity; + Current_ current; + Voltage_ voltage; + void calibrate(); + void idle(); + void position_mode(); + void velocity_mode(); + void current_mode(); + float set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint); + +}; diff --git a/include/tinymovr/current.hpp b/include/tinymovr/current.hpp new file mode 100644 index 0000000..a6e2ba6 --- /dev/null +++ b/include/tinymovr/current.hpp @@ -0,0 +1,33 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Current_ : Node +{ + public: + + Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_Iq_setpoint(void); + void set_Iq_setpoint(float value); + float get_Id_setpoint(void); + float get_Iq_limit(void); + void set_Iq_limit(float value); + float get_Iq_estimate(void); + float get_bandwidth(void); + void set_bandwidth(float value); + float get_Iq_p_gain(void); + float get_max_Ibus_regen(void); + void set_max_Ibus_regen(float value); + float get_max_Ibrake(void); + void set_max_Ibrake(float value); + +}; diff --git a/include/tinymovr/encoder.hpp b/include/tinymovr/encoder.hpp new file mode 100644 index 0000000..d05d2e4 --- /dev/null +++ b/include/tinymovr/encoder.hpp @@ -0,0 +1,28 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Encoder_ : Node +{ + public: + + Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_position_estimate(void); + float get_velocity_estimate(void); + uint8_t get_type(void); + void set_type(uint8_t value); + float get_bandwidth(void); + void set_bandwidth(float value); + bool get_calibrated(void); + uint8_t get_errors(void); + +}; diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp new file mode 100644 index 0000000..4428625 --- /dev/null +++ b/include/tinymovr/helpers.hpp @@ -0,0 +1,240 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#if defined ARDUINO || __cplusplus < 201103L +#include +#include +#else +#include +#include +#include +#endif + +#if defined ARDUINO +#include "Arduino.h" +#endif + +#define CAN_EP_SIZE (12) +#define CAN_EP_MASK ((1 << CAN_EP_SIZE) - 1) +#define CAN_SEQ_SIZE (9) +#define CAN_SEQ_MASK (((1 << CAN_SEQ_SIZE) - 1) << CAN_EP_SIZE) +#define CAN_DEV_SIZE (8) +#define CAN_DEV_MASK (((1 << CAN_DEV_SIZE) - 1) << (CAN_EP_SIZE + CAN_SEQ_SIZE)) + +typedef void (*send_callback)(uint32_t arbitration_id, uint8_t *data, uint8_t dlc, bool rtr); +typedef bool (*recv_callback)(uint32_t *arbitration_id, uint8_t *data, uint8_t *dlc); +typedef void (*delay_us_callback)(uint32_t us); + +class Node { + public: + + Node(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb), delay_us_cb(_delay_us_cb), delay_us_value(_delay_us_value) {} + + protected: + uint8_t can_node_id; + send_callback send_cb; + recv_callback recv_cb; + delay_us_callback delay_us_cb; + uint32_t delay_us_value; + uint8_t _data[8]; + uint8_t _dlc; + uint32_t get_arbitration_id(uint32_t cmd_id) + { + return ((this->can_node_id << (CAN_EP_SIZE + CAN_SEQ_SIZE)) & CAN_DEV_MASK) | (cmd_id & CAN_EP_MASK); + } + void send(uint32_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr) + { + const uint32_t arb_id = this->get_arbitration_id(cmd_id); + this->send_cb(arb_id, data, data_size, rtr); + } + + bool recv(uint32_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us) + { + uint32_t _arbitration_id; + uint8_t _data[8]; + uint8_t _data_size; + // A delay of a few 100s of us needs to be inserted + // to ensure the response has been transmitted. + // TODO: Better handle this using an interrupt. + if (delay_us > 0) + { + this->delay_us_cb(delay_us); + } + const uint32_t arb_id = this->get_arbitration_id(cmd_id); + while (this->recv_cb(&_arbitration_id, _data, &_data_size)) + { + if (_arbitration_id == arb_id) + { + memcpy(data, _data, _data_size); + *data_size = _data_size; + return true; + } + } + return false; + } +}; + +template +inline size_t write_le(T value, uint8_t* buffer); + +template +inline size_t read_le(T* value, const uint8_t* buffer); + +template<> +inline size_t write_le(bool value, uint8_t* buffer) { + buffer[0] = value ? 1 : 0; + return 1; +} + +template<> +inline size_t write_le(uint8_t value, uint8_t* buffer) { + buffer[0] = value; + return 1; +} + +template<> +inline size_t write_le(int8_t value, uint8_t* buffer) { + buffer[0] = value; + return 1; +} + +template<> +inline size_t write_le(uint16_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + return 2; +} + +template<> +inline size_t write_le(int16_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + return 2; +} + +template<> +inline size_t write_le(uint32_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + buffer[2] = (value >> 16) & 0xff; + buffer[3] = (value >> 24) & 0xff; + return 4; +} + +template<> +inline size_t write_le(int32_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + buffer[2] = (value >> 16) & 0xff; + buffer[3] = (value >> 24) & 0xff; + return 4; +} + +template<> +inline size_t write_le(uint64_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + buffer[2] = (value >> 16) & 0xff; + buffer[3] = (value >> 24) & 0xff; + buffer[4] = (value >> 32) & 0xff; + buffer[5] = (value >> 40) & 0xff; + buffer[6] = (value >> 48) & 0xff; + buffer[7] = (value >> 56) & 0xff; + return 8; +} + +template<> +inline size_t write_le(float value, uint8_t* buffer) { + //static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + //static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + const uint32_t * value_as_uint32 = reinterpret_cast(&value); + return write_le(*value_as_uint32, buffer); +} + +template<> +inline size_t read_le(bool* value, const uint8_t* buffer) { + *value = buffer[0]; + return 1; +} + +template<> +inline size_t read_le(uint8_t* value, const uint8_t* buffer) { + *value = buffer[0]; + return 1; +} + +template<> +inline size_t read_le(int8_t* value, const uint8_t* buffer) { + *value = buffer[0]; + return 1; +} + +template<> +inline size_t read_le(uint16_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8); + return 2; +} + +template<> +inline size_t read_le(int16_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8); + return 2; +} + +template<> +inline size_t read_le(int32_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24); + return 4; +} + +template<> +inline size_t read_le(uint32_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24); + return 4; +} + +template<> +inline size_t read_le(uint64_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24) | + (static_cast(buffer[4]) << 32) | + (static_cast(buffer[5]) << 40) | + (static_cast(buffer[6]) << 48) | + (static_cast(buffer[7]) << 56); + return 8; +} + +template<> +inline size_t read_le(float* value, const uint8_t* buffer) { + return read_le(reinterpret_cast(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 +static inline T read_le(const uint8_t** buffer, size_t* length) { + T result; + size_t cnt = read_le(&result, *buffer); + *buffer += cnt; + *length -= cnt; + return result; +} diff --git a/include/tinymovr/homing.hpp b/include/tinymovr/homing.hpp new file mode 100644 index 0000000..1f74286 --- /dev/null +++ b/include/tinymovr/homing.hpp @@ -0,0 +1,31 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include +#include + +class Homing_ : Node +{ + public: + + Homing_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , stall_detect(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_velocity(void); + void set_velocity(float value); + float get_max_homing_t(void); + void set_max_homing_t(float value); + float get_retract_dist(void); + void set_retract_dist(float value); + uint8_t get_warnings(void); + Stall_detect_ stall_detect; + void home(); + +}; diff --git a/include/tinymovr/motor.hpp b/include/tinymovr/motor.hpp new file mode 100644 index 0000000..ce298bf --- /dev/null +++ b/include/tinymovr/motor.hpp @@ -0,0 +1,36 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Motor_ : Node +{ + public: + + Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_R(void); + void set_R(float value); + float get_L(void); + void set_L(float value); + uint8_t get_pole_pairs(void); + void set_pole_pairs(uint8_t value); + uint8_t get_type(void); + void set_type(uint8_t value); + float get_offset(void); + void set_offset(float value); + int8_t get_direction(void); + void set_direction(int8_t value); + bool get_calibrated(void); + float get_I_cal(void); + void set_I_cal(float value); + uint8_t get_errors(void); + +}; diff --git a/include/tinymovr/position.hpp b/include/tinymovr/position.hpp new file mode 100644 index 0000000..3ea3937 --- /dev/null +++ b/include/tinymovr/position.hpp @@ -0,0 +1,24 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Position_ : Node +{ + public: + + Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_setpoint(void); + void set_setpoint(float value); + float get_p_gain(void); + void set_p_gain(float value); + +}; diff --git a/include/tinymovr/scheduler.hpp b/include/tinymovr/scheduler.hpp new file mode 100644 index 0000000..2fe9bbc --- /dev/null +++ b/include/tinymovr/scheduler.hpp @@ -0,0 +1,21 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Scheduler_ : Node +{ + public: + + Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + uint8_t get_errors(void); + +}; diff --git a/include/tinymovr/stall_detect.hpp b/include/tinymovr/stall_detect.hpp new file mode 100644 index 0000000..9088c58 --- /dev/null +++ b/include/tinymovr/stall_detect.hpp @@ -0,0 +1,26 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Stall_detect_ : Node +{ + public: + + Stall_detect_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_velocity(void); + void set_velocity(float value); + float get_delta_pos(void); + void set_delta_pos(float value); + float get_t(void); + void set_t(float value); + +}; diff --git a/include/tinymovr/tinymovr.hpp b/include/tinymovr/tinymovr.hpp new file mode 100644 index 0000000..400e186 --- /dev/null +++ b/include/tinymovr/tinymovr.hpp @@ -0,0 +1,132 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static uint32_t avlos_proto_hash = 4118115615; + +enum errors_flags +{ + ERRORS_NONE = 0, + ERRORS_UNDERVOLTAGE = (1 << 0), + ERRORS_DRIVER_FAULT = (1 << 1), + ERRORS_CHARGE_PUMP_FAULT_STAT = (1 << 2), + ERRORS_CHARGE_PUMP_FAULT = (1 << 3), + ERRORS_DRV10_DISABLE = (1 << 4), + ERRORS_DRV32_DISABLE = (1 << 5), + ERRORS_DRV54_DISABLE = (1 << 6) +}; + +enum scheduler_errors_flags +{ + SCHEDULER_ERRORS_NONE = 0, + SCHEDULER_ERRORS_CONTROL_BLOCK_REENTERED = (1 << 0) +}; + +enum controller_warnings_flags +{ + CONTROLLER_WARNINGS_NONE = 0, + CONTROLLER_WARNINGS_VELOCITY_LIMITED = (1 << 0), + CONTROLLER_WARNINGS_CURRENT_LIMITED = (1 << 1), + CONTROLLER_WARNINGS_MODULATION_LIMITED = (1 << 2) +}; + +enum controller_errors_flags +{ + CONTROLLER_ERRORS_NONE = 0, + CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED = (1 << 0) +}; + +enum motor_errors_flags +{ + MOTOR_ERRORS_NONE = 0, + MOTOR_ERRORS_PHASE_RESISTANCE_OUT_OF_RANGE = (1 << 0), + MOTOR_ERRORS_PHASE_INDUCTANCE_OUT_OF_RANGE = (1 << 1), + MOTOR_ERRORS_INVALID_POLE_PAIRS = (1 << 2) +}; + +enum encoder_errors_flags +{ + ENCODER_ERRORS_NONE = 0, + ENCODER_ERRORS_CALIBRATION_FAILED = (1 << 0), + ENCODER_ERRORS_READING_UNSTABLE = (1 << 1) +}; + +enum traj_planner_errors_flags +{ + TRAJ_PLANNER_ERRORS_NONE = 0, + TRAJ_PLANNER_ERRORS_INVALID_INPUT = (1 << 0), + TRAJ_PLANNER_ERRORS_VCRUISE_OVER_LIMIT = (1 << 1) +}; + +enum homing_warnings_flags +{ + HOMING_WARNINGS_NONE = 0, + HOMING_WARNINGS_HOMING_TIMEOUT = (1 << 0) +}; + +enum motor_type_options +{ + MOTOR_TYPE_HIGH_CURRENT = 0, + MOTOR_TYPE_GIMBAL = 1 +}; + +enum encoder_type_options +{ + ENCODER_TYPE_INTERNAL = 0, + ENCODER_TYPE_HALL = 1 +}; + +class Tinymovr : Node +{ + public: + + Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , motor(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , encoder(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , traj_planner(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , homing(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) + , watchdog(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + uint32_t get_protocol_hash(void); + uint32_t get_uid(void); + void get_fw_version(char out_value[]); + uint32_t get_hw_revision(void); + float get_Vbus(void); + float get_Ibus(void); + float get_power(void); + float get_temp(void); + bool get_calibrated(void); + uint8_t get_errors(void); + void save_config(); + void erase_config(); + void reset(); + void enter_dfu(); + Scheduler_ scheduler; + Controller_ controller; + Comms_ comms; + Motor_ motor; + Encoder_ encoder; + Traj_planner_ traj_planner; + Homing_ homing; + Watchdog_ watchdog; + +}; diff --git a/include/tinymovr/traj_planner.hpp b/include/tinymovr/traj_planner.hpp new file mode 100644 index 0000000..e4b96a4 --- /dev/null +++ b/include/tinymovr/traj_planner.hpp @@ -0,0 +1,35 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Traj_planner_ : Node +{ + public: + + Traj_planner_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_max_accel(void); + void set_max_accel(float value); + float get_max_decel(void); + void set_max_decel(float value); + float get_max_vel(void); + void set_max_vel(float value); + float get_t_accel(void); + void set_t_accel(float value); + float get_t_decel(void); + void set_t_decel(float value); + float get_t_total(void); + void set_t_total(float value); + void move_to(float pos_setpoint); + void move_to_tlimit(float pos_setpoint); + uint8_t get_errors(void); + +}; diff --git a/include/tinymovr/velocity.hpp b/include/tinymovr/velocity.hpp new file mode 100644 index 0000000..e9fb5c0 --- /dev/null +++ b/include/tinymovr/velocity.hpp @@ -0,0 +1,32 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Velocity_ : Node +{ + public: + + Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_setpoint(void); + void set_setpoint(float value); + float get_limit(void); + void set_limit(float value); + float get_p_gain(void); + void set_p_gain(float value); + float get_i_gain(void); + void set_i_gain(float value); + float get_deadband(void); + void set_deadband(float value); + float get_increment(void); + void set_increment(float value); + +}; diff --git a/include/tinymovr/voltage.hpp b/include/tinymovr/voltage.hpp new file mode 100644 index 0000000..661bb4e --- /dev/null +++ b/include/tinymovr/voltage.hpp @@ -0,0 +1,21 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Voltage_ : Node +{ + public: + + Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + float get_Vq_setpoint(void); + +}; diff --git a/include/tinymovr/watchdog.hpp b/include/tinymovr/watchdog.hpp new file mode 100644 index 0000000..538eb1e --- /dev/null +++ b/include/tinymovr/watchdog.hpp @@ -0,0 +1,25 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#pragma once + +#include + +class Watchdog_ : Node +{ + public: + + Watchdog_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb, uint32_t _delay_us_value): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb, _delay_us_value) {}; + bool get_enabled(void); + void set_enabled(bool value); + bool get_triggered(void); + float get_timeout(void); + void set_timeout(float value); + +}; diff --git a/include/tinymovr_ros/diffbot_hw_iface.hpp b/include/tinymovr_ros/diffbot_hw_iface.hpp deleted file mode 100644 index 5938e10..0000000 --- a/include/tinymovr_ros/diffbot_hw_iface.hpp +++ /dev/null @@ -1,38 +0,0 @@ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace tinymovr_ros -{ - -class Diffbot : public hardware_interface::RobotHW -{ -public: - Diffbot(); - - bool read(const ros::Duration& period); - bool write(); - -private: - - const double radius = 0.075; // radius of the wheels - const double dist_w = 0.38; // distance between the wheels - - TinymovrCAN tmcan; - - hardware_interface::JointStateInterface jnt_state_interface; - hardware_interface::VelocityJointInterface jnt_vel_interface; - std::vector hw_commands_ {0.0, 0.0}; // rad/s - std::vector hw_positions_ {0.0, 0.0}; // rad - std::vector hw_velocities_ {0.0, 0.0}; // rad/s - std::vector hw_efforts_ {0.0, 0.0}; // Nm? - std::vector hw_node_ids_ {1, 2}; -}; - -} \ No newline at end of file diff --git a/include/tinymovr_ros/tinymovr_can.hpp b/include/tinymovr_ros/tinymovr_can.hpp deleted file mode 100644 index 4063fd3..0000000 --- a/include/tinymovr_ros/tinymovr_can.hpp +++ /dev/null @@ -1,21 +0,0 @@ - -#pragma once - -#include "socketcan_cpp/socketcan_cpp.hpp" -#include - -namespace tinymovr_ros -{ - -class TinymovrCAN -{ -public: - void init(); - bool read_frame(uint32_t node_id, uint32_t ep_id, uint8_t* data, uint8_t* data_len); - bool write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len); - uint32_t make_arbitration_id(uint32_t node_id, uint32_t command_id); -private: - scpp::SocketCan socket_can; -}; - -} diff --git a/include/tinymovr_ros/tm_hw_iface.hpp b/include/tinymovr_ros/tm_hw_iface.hpp deleted file mode 100644 index b770c90..0000000 --- a/include/tinymovr_ros/tm_hw_iface.hpp +++ /dev/null @@ -1,53 +0,0 @@ - -#pragma once - -#include -#include -#include -#include - -namespace tinymovr_ros -{ - -class Tinymovr : public hardware_interface::RobotHW -{ -public: - Tinymovr() - { - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle("State", &pos, &vel, &eff); - jnt_state_interface.registerHandle(state_handle); - - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle pos_handle(jnt_state_interface.getHandle("State"), &cmd_pos); - jnt_pos_interface.registerHandle(pos_handle); - - // connect and register the joint velocity interface - hardware_interface::JointHandle vel_handle(jnt_state_interface.getHandle("State"), &cmd_vel); - jnt_vel_interface.registerHandle(vel_handle); - - // connect and register the joint effort interface - hardware_interface::JointHandle eff_handle(jnt_state_interface.getHandle("State"), &cmd_eff); - jnt_eff_interface.registerHandle(eff_handle); - - registerInterface(&jnt_pos_interface); - } - bool read(const ros::Duration& period); - bool write(); - -private: - hardware_interface::JointStateInterface jnt_state_interface; - hardware_interface::PositionJointInterface jnt_pos_interface; - hardware_interface::VelocityJointInterface jnt_vel_interface; - hardware_interface::EffortJointInterface jnt_eff_interface; - double cmd_pos; - double cmd_vel; - double cmd_eff; - double pos; - double vel; - double eff; -}; - -} \ No newline at end of file diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp new file mode 100644 index 0000000..6c6d18b --- /dev/null +++ b/include/tm_joint_iface.hpp @@ -0,0 +1,56 @@ + +#pragma once + +// ROS +#include +#include +#include + +#include +#include +#include + +#include + +using namespace std; + +namespace tinymovr_ros +{ + +class TinymovrJoint : public hardware_interface::RobotHW +{ +public: + TinymovrJoint(); + ~TinymovrJoint(); + bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh); + void read(const ros::Time& /*time*/, const ros::Duration& /*period*/); + void write(const ros::Time& /*time*/, const ros::Duration& /*period*/); + void shutdown() ; + +protected: + ros::NodeHandle nh_; + + hardware_interface::JointStateInterface joint_state_interface; + hardware_interface::PositionJointInterface joint_pos_interface; + hardware_interface::VelocityJointInterface joint_vel_interface; + hardware_interface::EffortJointInterface joint_eff_interface; + + int num_joints; + std::vector joint_name; + std::vector joint_id; + std::vector rads_to_ticks; + + std::vector joint_position_command; + std::vector joint_velocity_command; + std::vector joint_effort_command; + std::vector joint_position_state; + std::vector joint_velocity_state; + std::vector joint_effort_state; + + std::vector servos; + std::vector servo_modes; + + uint8_t _str2mode(std::string& mode_string); +}; + +} \ No newline at end of file diff --git a/launch/diffbot_ros_node.launch b/launch/diffbot_ros_node.launch deleted file mode 100644 index 5b1e1d3..0000000 --- a/launch/diffbot_ros_node.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/launch/tinymovr_diffbot_demo_node.launch b/launch/tinymovr_diffbot_demo_node.launch new file mode 100644 index 0000000..d39ab7c --- /dev/null +++ b/launch/tinymovr_diffbot_demo_node.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/tinymovr_ros_node.launch b/launch/tinymovr_ros_node.launch deleted file mode 100644 index 1155467..0000000 --- a/launch/tinymovr_ros_node.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/package.xml b/package.xml index 3274eb6..23d2f8c 100644 --- a/package.xml +++ b/package.xml @@ -49,6 +49,11 @@ catkin + + velocity_controllers + position_controllers + effort_controllers + controller_interface controller_manager geometry_msgs @@ -81,6 +86,7 @@ std_msgs tf sensor_msgs + diff_drive_controller diff --git a/rviz/diffbot.rviz b/rviz/diffbot.rviz deleted file mode 100644 index ce68983..0000000 --- a/rviz/diffbot.rviz +++ /dev/null @@ -1,174 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Axes1 - Splitter Ratio: 0.5 - Tree Height: 549 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/Axes - Enabled: true - Length: 9.999999747378752e-5 - Name: Axes - Radius: 0.004999999888241291 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fwheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fwheel_attach: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - lwheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rwheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - scanner: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 1.6136374473571777 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.545397937297821 - Target Frame: - Value: Orbit (rviz) - Yaw: 2.0203981399536133 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1267 - X: 67 - Y: 30 diff --git a/src/diffbot_hw_iface.cpp b/src/diffbot_hw_iface.cpp deleted file mode 100644 index 37041ee..0000000 --- a/src/diffbot_hw_iface.cpp +++ /dev/null @@ -1,74 +0,0 @@ - -#include -#include -#include - -#include -#include -#include - -using namespace tinymovr_ros; - -#define CMD_GET_ENC_ESTIMATES 0x1A -#define CMD_SET_VEL 0x1B - -Diffbot::Diffbot() -{ - tmcan.init(); - - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_a("A", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); - jnt_state_interface.registerHandle(state_handle_a); - - hardware_interface::JointStateHandle state_handle_b("B", &hw_positions_[1], &hw_velocities_[1], &hw_efforts_[1]); - jnt_state_interface.registerHandle(state_handle_b); - - registerInterface(&jnt_state_interface); - - // connect and register the joint velocity interface - hardware_interface::JointHandle vel_handle_a(jnt_state_interface.getHandle("A"), &hw_commands_[0]); - jnt_vel_interface.registerHandle(vel_handle_a); - - hardware_interface::JointHandle vel_handle_b(jnt_state_interface.getHandle("B"), &hw_commands_[1]); - jnt_vel_interface.registerHandle(vel_handle_b); - - registerInterface(&jnt_vel_interface); -} - -bool Diffbot::read(const ros::Duration& dt) -{ - uint8_t data[8] = {0}; - uint8_t data_len = 0; - - for (uint i = 0; i < hw_commands_.size(); i++) - { - if (!tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_len)) - { - return false; - } - - // TODO: assert data length == 8 - - const float* hw_pos_float = reinterpret_cast(data); - const float* hw_vel_float = reinterpret_cast(data+4); - - hw_positions_[i] = (double)(*hw_pos_float); - hw_velocities_[i] = (double)(*hw_vel_float); - } - - return true; -} - -bool Diffbot::write() -{ - for (uint i = 0; i < hw_commands_.size(); i++) - { - const float command = hw_commands_[i]; - const uint8_t *data = reinterpret_cast(&command); - if (!tmcan.write_frame(hw_node_ids_[i], CMD_SET_VEL, data, sizeof(float))) - { - return false; - } - } - return true; -} diff --git a/src/diffbot_hw_iface_node.cpp b/src/diffbot_hw_iface_node.cpp deleted file mode 100644 index 5d0a70c..0000000 --- a/src/diffbot_hw_iface_node.cpp +++ /dev/null @@ -1,31 +0,0 @@ - -#include -#include - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "tinymovr_ros_diffbot_iface_node"); - - tinymovr_ros::Diffbot db; - ros::NodeHandle nh; - controller_manager::ControllerManager cm(&db, nh); - - ros::Rate rate(20.0); - ros::Time last; - - ros::AsyncSpinner spinner(2); - spinner.start(); - - while (ros::ok()) - { - const ros::Time now = ros::Time::now(); - const ros::Duration period = now - last; - last = now; - db.read(period); - cm.update(now, period); - db.write(); - rate.sleep(); - } - spinner.stop(); - return 0; -} diff --git a/src/socketcan_cpp.cpp b/src/socketcan_cpp/socketcan_cpp.cpp similarity index 91% rename from src/socketcan_cpp.cpp rename to src/socketcan_cpp/socketcan_cpp.cpp index 1084a68..a62b5ad 100644 --- a/src/socketcan_cpp.cpp +++ b/src/socketcan_cpp/socketcan_cpp.cpp @@ -49,9 +49,8 @@ unsigned char can_len2dlc(unsigned char len) namespace scpp { - SocketCan::SocketCan() - { - } + SocketCan::SocketCan() {} + SocketCanStatus SocketCan::open(const std::string & can_interface, int32_t read_timeout_ms, SocketMode mode) { m_interface = can_interface; @@ -128,11 +127,19 @@ namespace scpp perror("bind"); return STATUS_BIND_ERROR; } + struct can_filter rfilter[1]; + rfilter[0].can_id = ~0x00000700; + rfilter[0].can_mask = 0x1FFFFF00; + if (setsockopt(m_socket, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)) < 0) { + perror("setsockopt"); + return STATUS_SOCKET_CREATE_ERROR; // You should define this status + } #else printf("Your operating system does not support socket can! \n"); #endif return STATUS_OK; } + SocketCanStatus SocketCan::write(const CanFrame & msg) { #ifdef HAVE_SOCKETCAN_HEADERS @@ -140,6 +147,9 @@ namespace scpp memset(&frame, 0, sizeof(frame)); /* init CAN FD frame, e.g. LEN = 0 */ //convert CanFrame to canfd_frame frame.can_id = msg.id; + if (msg.id > 0x7FF) { + frame.can_id |= CAN_EFF_FLAG; // Set extended frame format bit + } frame.len = msg.len; frame.flags = msg.flags; memcpy(frame.data, msg.data, msg.len); @@ -159,19 +169,17 @@ namespace scpp #endif return STATUS_OK; } + SocketCanStatus SocketCan::read(CanFrame & msg) { #ifdef HAVE_SOCKETCAN_HEADERS struct canfd_frame frame; - - // Read in a CAN frame auto num_bytes = ::read(m_socket, &frame, CANFD_MTU); if (num_bytes != CAN_MTU && num_bytes != CANFD_MTU) { - //perror("Can read error"); + perror("Can read error"); return STATUS_READ_ERROR; } - msg.id = frame.can_id; msg.len = frame.len; msg.flags = frame.flags; @@ -181,6 +189,7 @@ namespace scpp #endif return STATUS_OK; } + SocketCanStatus SocketCan::close() { #ifdef HAVE_SOCKETCAN_HEADERS @@ -192,8 +201,9 @@ namespace scpp { return m_interface; } + SocketCan::~SocketCan() { close(); } -} +} \ No newline at end of file diff --git a/src/tinymovr/can.cpp b/src/tinymovr/can.cpp new file mode 100644 index 0000000..33b1da0 --- /dev/null +++ b/src/tinymovr/can.cpp @@ -0,0 +1,46 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +uint32_t Can_::get_rate(void) +{ + uint32_t value = 0; + this->send(42, this->_data, 0, true); + if (this->recv(42, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Can_::set_rate(uint32_t value) +{ + write_le(value, this->_data); + this->send(42, this->_data, sizeof(uint32_t), false); +} + +uint32_t Can_::get_id(void) +{ + uint32_t value = 0; + this->send(43, this->_data, 0, true); + if (this->recv(43, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Can_::set_id(uint32_t value) +{ + write_le(value, this->_data); + this->send(43, this->_data, sizeof(uint32_t), false); +} + + + diff --git a/src/tinymovr/comms.cpp b/src/tinymovr/comms.cpp new file mode 100644 index 0000000..7005dcd --- /dev/null +++ b/src/tinymovr/comms.cpp @@ -0,0 +1,12 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + + + diff --git a/src/tinymovr/controller.cpp b/src/tinymovr/controller.cpp new file mode 100644 index 0000000..9bd6447 --- /dev/null +++ b/src/tinymovr/controller.cpp @@ -0,0 +1,111 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +uint8_t Controller_::get_state(void) +{ + uint8_t value = 0; + this->send(15, this->_data, 0, true); + if (this->recv(15, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Controller_::set_state(uint8_t value) +{ + write_le(value, this->_data); + this->send(15, this->_data, sizeof(uint8_t), false); +} + +uint8_t Controller_::get_mode(void) +{ + uint8_t value = 0; + this->send(16, this->_data, 0, true); + if (this->recv(16, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Controller_::set_mode(uint8_t value) +{ + write_le(value, this->_data); + this->send(16, this->_data, sizeof(uint8_t), false); +} + +uint8_t Controller_::get_warnings(void) +{ + uint8_t value = 0; + this->send(17, this->_data, 0, true); + if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +uint8_t Controller_::get_errors(void) +{ + uint8_t value = 0; + this->send(18, this->_data, 0, true); + if (this->recv(18, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + +void Controller_::calibrate() +{ + this->send(36, this->_data, 0, true); +} + +void Controller_::idle() +{ + this->send(37, this->_data, 0, true); +} + +void Controller_::position_mode() +{ + this->send(38, this->_data, 0, true); +} + +void Controller_::velocity_mode() +{ + this->send(39, this->_data, 0, true); +} + +void Controller_::current_mode() +{ + this->send(40, this->_data, 0, true); +} + +float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint) +{ + uint8_t data_len = 0; + write_le(pos_setpoint, this->_data + data_len); + data_len += sizeof(pos_setpoint); + write_le(vel_setpoint, this->_data + data_len); + data_len += sizeof(vel_setpoint); + + this->send(41, this->_data, data_len, false); + float value = 0; + this->send(17, this->_data, 0, true); + if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + diff --git a/src/tinymovr/current.cpp b/src/tinymovr/current.cpp new file mode 100644 index 0000000..c7873f8 --- /dev/null +++ b/src/tinymovr/current.cpp @@ -0,0 +1,130 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Current_::get_Iq_setpoint(void) +{ + float value = 0; + this->send(27, this->_data, 0, true); + if (this->recv(27, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_Iq_setpoint(float value) +{ + write_le(value, this->_data); + this->send(27, this->_data, sizeof(float), false); +} + +float Current_::get_Id_setpoint(void) +{ + float value = 0; + this->send(28, this->_data, 0, true); + if (this->recv(28, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +float Current_::get_Iq_limit(void) +{ + float value = 0; + this->send(29, this->_data, 0, true); + if (this->recv(29, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_Iq_limit(float value) +{ + write_le(value, this->_data); + this->send(29, this->_data, sizeof(float), false); +} + +float Current_::get_Iq_estimate(void) +{ + float value = 0; + this->send(30, this->_data, 0, true); + if (this->recv(30, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +float Current_::get_bandwidth(void) +{ + float value = 0; + this->send(31, this->_data, 0, true); + if (this->recv(31, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_bandwidth(float value) +{ + write_le(value, this->_data); + this->send(31, this->_data, sizeof(float), false); +} + +float Current_::get_Iq_p_gain(void) +{ + float value = 0; + this->send(32, this->_data, 0, true); + if (this->recv(32, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +float Current_::get_max_Ibus_regen(void) +{ + float value = 0; + this->send(33, this->_data, 0, true); + if (this->recv(33, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_max_Ibus_regen(float value) +{ + write_le(value, this->_data); + this->send(33, this->_data, sizeof(float), false); +} + +float Current_::get_max_Ibrake(void) +{ + float value = 0; + this->send(34, this->_data, 0, true); + if (this->recv(34, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_max_Ibrake(float value) +{ + write_le(value, this->_data); + this->send(34, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr/encoder.cpp b/src/tinymovr/encoder.cpp new file mode 100644 index 0000000..9622c48 --- /dev/null +++ b/src/tinymovr/encoder.cpp @@ -0,0 +1,90 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Encoder_::get_position_estimate(void) +{ + float value = 0; + this->send(53, this->_data, 0, true); + if (this->recv(53, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +float Encoder_::get_velocity_estimate(void) +{ + float value = 0; + this->send(54, this->_data, 0, true); + if (this->recv(54, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +uint8_t Encoder_::get_type(void) +{ + uint8_t value = 0; + this->send(55, this->_data, 0, true); + if (this->recv(55, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Encoder_::set_type(uint8_t value) +{ + write_le(value, this->_data); + this->send(55, this->_data, sizeof(uint8_t), false); +} + +float Encoder_::get_bandwidth(void) +{ + float value = 0; + this->send(56, this->_data, 0, true); + if (this->recv(56, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Encoder_::set_bandwidth(float value) +{ + write_le(value, this->_data); + this->send(56, this->_data, sizeof(float), false); +} + +bool Encoder_::get_calibrated(void) +{ + bool value = 0; + this->send(57, this->_data, 0, true); + if (this->recv(57, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +uint8_t Encoder_::get_errors(void) +{ + uint8_t value = 0; + this->send(58, this->_data, 0, true); + if (this->recv(58, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + + diff --git a/src/tinymovr/homing.cpp b/src/tinymovr/homing.cpp new file mode 100644 index 0000000..697b292 --- /dev/null +++ b/src/tinymovr/homing.cpp @@ -0,0 +1,79 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Homing_::get_velocity(void) +{ + float value = 0; + this->send(68, this->_data, 0, true); + if (this->recv(68, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Homing_::set_velocity(float value) +{ + write_le(value, this->_data); + this->send(68, this->_data, sizeof(float), false); +} + +float Homing_::get_max_homing_t(void) +{ + float value = 0; + this->send(69, this->_data, 0, true); + if (this->recv(69, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Homing_::set_max_homing_t(float value) +{ + write_le(value, this->_data); + this->send(69, this->_data, sizeof(float), false); +} + +float Homing_::get_retract_dist(void) +{ + float value = 0; + this->send(70, this->_data, 0, true); + if (this->recv(70, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Homing_::set_retract_dist(float value) +{ + write_le(value, this->_data); + this->send(70, this->_data, sizeof(float), false); +} + +uint8_t Homing_::get_warnings(void) +{ + uint8_t value = 0; + this->send(71, this->_data, 0, true); + if (this->recv(71, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + +void Homing_::home() +{ + this->send(75, this->_data, 0, true); +} + + diff --git a/src/tinymovr/motor.cpp b/src/tinymovr/motor.cpp new file mode 100644 index 0000000..2eb11bc --- /dev/null +++ b/src/tinymovr/motor.cpp @@ -0,0 +1,153 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Motor_::get_R(void) +{ + float value = 0; + this->send(44, this->_data, 0, true); + if (this->recv(44, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_R(float value) +{ + write_le(value, this->_data); + this->send(44, this->_data, sizeof(float), false); +} + +float Motor_::get_L(void) +{ + float value = 0; + this->send(45, this->_data, 0, true); + if (this->recv(45, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_L(float value) +{ + write_le(value, this->_data); + this->send(45, this->_data, sizeof(float), false); +} + +uint8_t Motor_::get_pole_pairs(void) +{ + uint8_t value = 0; + this->send(46, this->_data, 0, true); + if (this->recv(46, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_pole_pairs(uint8_t value) +{ + write_le(value, this->_data); + this->send(46, this->_data, sizeof(uint8_t), false); +} + +uint8_t Motor_::get_type(void) +{ + uint8_t value = 0; + this->send(47, this->_data, 0, true); + if (this->recv(47, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_type(uint8_t value) +{ + write_le(value, this->_data); + this->send(47, this->_data, sizeof(uint8_t), false); +} + +float Motor_::get_offset(void) +{ + float value = 0; + this->send(48, this->_data, 0, true); + if (this->recv(48, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_offset(float value) +{ + write_le(value, this->_data); + this->send(48, this->_data, sizeof(float), false); +} + +int8_t Motor_::get_direction(void) +{ + int8_t value = 0; + this->send(49, this->_data, 0, true); + if (this->recv(49, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_direction(int8_t value) +{ + write_le(value, this->_data); + this->send(49, this->_data, sizeof(int8_t), false); +} + +bool Motor_::get_calibrated(void) +{ + bool value = 0; + this->send(50, this->_data, 0, true); + if (this->recv(50, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +float Motor_::get_I_cal(void) +{ + float value = 0; + this->send(51, this->_data, 0, true); + if (this->recv(51, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_I_cal(float value) +{ + write_le(value, this->_data); + this->send(51, this->_data, sizeof(float), false); +} + +uint8_t Motor_::get_errors(void) +{ + uint8_t value = 0; + this->send(52, this->_data, 0, true); + if (this->recv(52, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + + diff --git a/src/tinymovr/position.cpp b/src/tinymovr/position.cpp new file mode 100644 index 0000000..6848657 --- /dev/null +++ b/src/tinymovr/position.cpp @@ -0,0 +1,46 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Position_::get_setpoint(void) +{ + float value = 0; + this->send(19, this->_data, 0, true); + if (this->recv(19, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Position_::set_setpoint(float value) +{ + write_le(value, this->_data); + this->send(19, this->_data, sizeof(float), false); +} + +float Position_::get_p_gain(void) +{ + float value = 0; + this->send(20, this->_data, 0, true); + if (this->recv(20, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Position_::set_p_gain(float value) +{ + write_le(value, this->_data); + this->send(20, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr/scheduler.cpp b/src/tinymovr/scheduler.cpp new file mode 100644 index 0000000..de9bd30 --- /dev/null +++ b/src/tinymovr/scheduler.cpp @@ -0,0 +1,23 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +uint8_t Scheduler_::get_errors(void) +{ + uint8_t value = 0; + this->send(14, this->_data, 0, true); + if (this->recv(14, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + + diff --git a/src/tinymovr/stall_detect.cpp b/src/tinymovr/stall_detect.cpp new file mode 100644 index 0000000..b3a5d93 --- /dev/null +++ b/src/tinymovr/stall_detect.cpp @@ -0,0 +1,63 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Stall_detect_::get_velocity(void) +{ + float value = 0; + this->send(72, this->_data, 0, true); + if (this->recv(72, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Stall_detect_::set_velocity(float value) +{ + write_le(value, this->_data); + this->send(72, this->_data, sizeof(float), false); +} + +float Stall_detect_::get_delta_pos(void) +{ + float value = 0; + this->send(73, this->_data, 0, true); + if (this->recv(73, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Stall_detect_::set_delta_pos(float value) +{ + write_le(value, this->_data); + this->send(73, this->_data, sizeof(float), false); +} + +float Stall_detect_::get_t(void) +{ + float value = 0; + this->send(74, this->_data, 0, true); + if (this->recv(74, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Stall_detect_::set_t(float value) +{ + write_le(value, this->_data); + this->send(74, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr/tinymovr.cpp b/src/tinymovr/tinymovr.cpp new file mode 100644 index 0000000..f4e977b --- /dev/null +++ b/src/tinymovr/tinymovr.cpp @@ -0,0 +1,128 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ +#include +uint32_t Tinymovr::get_protocol_hash(void) +{ + uint32_t value = 0; + this->send(0, this->_data, 0, true); + if (this->recv(0, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +uint32_t Tinymovr::get_uid(void) +{ + uint32_t value = 0; + this->send(1, this->_data, 0, true); + if (this->recv(1, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +void Tinymovr::get_fw_version(char out_value[]) +{ + this->send(2, this->_data, 0, true); + this->_dlc = 0; + if (this->recv(2, this->_data, &(this->_dlc), this->delay_us_value)) + { + memcpy(out_value, this->_data, this->_dlc); + } +} +uint32_t Tinymovr::get_hw_revision(void) +{ + uint32_t value = 0; + this->send(3, this->_data, 0, true); + if (this->recv(3, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_Vbus(void) +{ + float value = 0; + this->send(4, this->_data, 0, true); + if (this->recv(4, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_Ibus(void) +{ + float value = 0; + this->send(5, this->_data, 0, true); + if (this->recv(5, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_power(void) +{ + float value = 0; + this->send(6, this->_data, 0, true); + if (this->recv(6, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_temp(void) +{ + float value = 0; + this->send(7, this->_data, 0, true); + if (this->recv(7, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +bool Tinymovr::get_calibrated(void) +{ + bool value = 0; + this->send(8, this->_data, 0, true); + if (this->recv(8, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} +uint8_t Tinymovr::get_errors(void) +{ + uint8_t value = 0; + this->send(9, this->_data, 0, true); + if (this->recv(9, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Tinymovr::save_config() +{ + this->send(10, this->_data, 0, true); +} + +void Tinymovr::erase_config() +{ + this->send(11, this->_data, 0, true); +} + +void Tinymovr::reset() +{ + this->send(12, this->_data, 0, true); +} + +void Tinymovr::enter_dfu() +{ + this->send(13, this->_data, 0, true); +} + diff --git a/src/tinymovr/traj_planner.cpp b/src/tinymovr/traj_planner.cpp new file mode 100644 index 0000000..058ec08 --- /dev/null +++ b/src/tinymovr/traj_planner.cpp @@ -0,0 +1,143 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Traj_planner_::get_max_accel(void) +{ + float value = 0; + this->send(59, this->_data, 0, true); + if (this->recv(59, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_max_accel(float value) +{ + write_le(value, this->_data); + this->send(59, this->_data, sizeof(float), false); +} + +float Traj_planner_::get_max_decel(void) +{ + float value = 0; + this->send(60, this->_data, 0, true); + if (this->recv(60, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_max_decel(float value) +{ + write_le(value, this->_data); + this->send(60, this->_data, sizeof(float), false); +} + +float Traj_planner_::get_max_vel(void) +{ + float value = 0; + this->send(61, this->_data, 0, true); + if (this->recv(61, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_max_vel(float value) +{ + write_le(value, this->_data); + this->send(61, this->_data, sizeof(float), false); +} + +float Traj_planner_::get_t_accel(void) +{ + float value = 0; + this->send(62, this->_data, 0, true); + if (this->recv(62, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_t_accel(float value) +{ + write_le(value, this->_data); + this->send(62, this->_data, sizeof(float), false); +} + +float Traj_planner_::get_t_decel(void) +{ + float value = 0; + this->send(63, this->_data, 0, true); + if (this->recv(63, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_t_decel(float value) +{ + write_le(value, this->_data); + this->send(63, this->_data, sizeof(float), false); +} + +float Traj_planner_::get_t_total(void) +{ + float value = 0; + this->send(64, this->_data, 0, true); + if (this->recv(64, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_t_total(float value) +{ + write_le(value, this->_data); + this->send(64, this->_data, sizeof(float), false); +} + + +void Traj_planner_::move_to(float pos_setpoint) +{ + uint8_t data_len = 0; + write_le(pos_setpoint, this->_data + data_len); + data_len += sizeof(pos_setpoint); + + this->send(65, this->_data, data_len, false); +} + +void Traj_planner_::move_to_tlimit(float pos_setpoint) +{ + uint8_t data_len = 0; + write_le(pos_setpoint, this->_data + data_len); + data_len += sizeof(pos_setpoint); + + this->send(66, this->_data, data_len, false); +} +uint8_t Traj_planner_::get_errors(void) +{ + uint8_t value = 0; + this->send(67, this->_data, 0, true); + if (this->recv(67, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + + diff --git a/src/tinymovr/velocity.cpp b/src/tinymovr/velocity.cpp new file mode 100644 index 0000000..318995e --- /dev/null +++ b/src/tinymovr/velocity.cpp @@ -0,0 +1,114 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Velocity_::get_setpoint(void) +{ + float value = 0; + this->send(21, this->_data, 0, true); + if (this->recv(21, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_setpoint(float value) +{ + write_le(value, this->_data); + this->send(21, this->_data, sizeof(float), false); +} + +float Velocity_::get_limit(void) +{ + float value = 0; + this->send(22, this->_data, 0, true); + if (this->recv(22, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_limit(float value) +{ + write_le(value, this->_data); + this->send(22, this->_data, sizeof(float), false); +} + +float Velocity_::get_p_gain(void) +{ + float value = 0; + this->send(23, this->_data, 0, true); + if (this->recv(23, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_p_gain(float value) +{ + write_le(value, this->_data); + this->send(23, this->_data, sizeof(float), false); +} + +float Velocity_::get_i_gain(void) +{ + float value = 0; + this->send(24, this->_data, 0, true); + if (this->recv(24, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_i_gain(float value) +{ + write_le(value, this->_data); + this->send(24, this->_data, sizeof(float), false); +} + +float Velocity_::get_deadband(void) +{ + float value = 0; + this->send(25, this->_data, 0, true); + if (this->recv(25, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_deadband(float value) +{ + write_le(value, this->_data); + this->send(25, this->_data, sizeof(float), false); +} + +float Velocity_::get_increment(void) +{ + float value = 0; + this->send(26, this->_data, 0, true); + if (this->recv(26, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_increment(float value) +{ + write_le(value, this->_data); + this->send(26, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr/voltage.cpp b/src/tinymovr/voltage.cpp new file mode 100644 index 0000000..754640b --- /dev/null +++ b/src/tinymovr/voltage.cpp @@ -0,0 +1,23 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +float Voltage_::get_Vq_setpoint(void) +{ + float value = 0; + this->send(35, this->_data, 0, true); + if (this->recv(35, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + + + diff --git a/src/tinymovr/watchdog.cpp b/src/tinymovr/watchdog.cpp new file mode 100644 index 0000000..35548b2 --- /dev/null +++ b/src/tinymovr/watchdog.cpp @@ -0,0 +1,57 @@ +/* +* This file was automatically generated using Avlos. +* https://github.com/tinymovr/avlos +* +* Any changes to this file will be overwritten when +* content is regenerated. +*/ + +#include + +bool Watchdog_::get_enabled(void) +{ + bool value = 0; + this->send(76, this->_data, 0, true); + if (this->recv(76, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Watchdog_::set_enabled(bool value) +{ + write_le(value, this->_data); + this->send(76, this->_data, sizeof(bool), false); +} + +bool Watchdog_::get_triggered(void) +{ + bool value = 0; + this->send(77, this->_data, 0, true); + if (this->recv(77, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +float Watchdog_::get_timeout(void) +{ + float value = 0; + this->send(78, this->_data, 0, true); + if (this->recv(78, this->_data, &(this->_dlc), this->delay_us_value)) + { + read_le(&value, this->_data); + } + return value; +} + +void Watchdog_::set_timeout(float value) +{ + write_le(value, this->_data); + this->send(78, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp deleted file mode 100644 index b186bb6..0000000 --- a/src/tinymovr_can.cpp +++ /dev/null @@ -1,60 +0,0 @@ - -#include - -namespace tinymovr_ros -{ - -void TinymovrCAN::init() -{ - auto status = socket_can.open("can0"); - if (scpp::STATUS_OK == status) - { - ROS_INFO("Socketcan opened successfully"); - } - else - { - ROS_ERROR("Cant' open Socketcan: %d", status); - exit(1); - } -} - -bool TinymovrCAN::read_frame(uint32_t node_id, uint32_t ep_id, uint8_t* data, uint8_t* data_len) -{ - if (scpp::STATUS_OK != write_frame(node_id, ep_id, 0, 0)) - { - return false; - } - - scpp::CanFrame fr; - - if (scpp::STATUS_OK != socket_can.read(fr)) - { - return false; - } - - memcpy(data, fr.data, 8); // TODO: make safer - *data_len = fr.len; - - return true; -} - -bool TinymovrCAN::write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len) -{ - scpp::CanFrame cf_to_write; - - cf_to_write.id = make_arbitration_id(node_id, ep_id); - cf_to_write.len = data_len; - for (int i = 0; i < data_len; ++i) - cf_to_write.data[i] = data[i]; - auto write_sc_status = socket_can.write(cf_to_write); - if (write_sc_status != scpp::STATUS_OK) - return false; - return true; -} - -uint32_t TinymovrCAN::make_arbitration_id(uint32_t node_id, uint32_t command_id) -{ - return node_id << 6 | command_id; -} - -} \ No newline at end of file diff --git a/src/tm_hw_iface.cpp b/src/tm_hw_iface.cpp deleted file mode 100644 index 2919d0f..0000000 --- a/src/tm_hw_iface.cpp +++ /dev/null @@ -1,14 +0,0 @@ - -#include - -using namespace tinymovr_ros; - -bool Tinymovr::read(const ros::Duration& dt) -{ - return true; -} - -bool Tinymovr::write() -{ - return true; -} \ No newline at end of file diff --git a/src/tm_hw_iface_node.cpp b/src/tm_hw_iface_node.cpp deleted file mode 100644 index 6cba2f4..0000000 --- a/src/tm_hw_iface_node.cpp +++ /dev/null @@ -1,31 +0,0 @@ - -#include -#include - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "tinymovr_ros_iface_node"); - - tinymovr_ros::Tinymovr tm; - ros::NodeHandle nh; - controller_manager::ControllerManager cm(&tm, nh); - - ros::Time last, now; - now = last = ros::Time::now(); - ros::Duration period(0.05); - - ros::AsyncSpinner spinner(2); - spinner.start(); - - while (ros::ok()) - { - now = ros::Time::now(); - period = now - last; - last = now; - tm.read(period); - cm.update(now, period); - tm.write(); - } - spinner.stop(); - return 0; -} diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp new file mode 100644 index 0000000..4902874 --- /dev/null +++ b/src/tm_joint_iface.cpp @@ -0,0 +1,378 @@ + +#include +#include +#include + +#include +#include +#include + +#include + +#include "socketcan_cpp/socketcan_cpp.hpp" +#include + +using namespace std; + +namespace tinymovr_ros +{ + +scpp::SocketCan socket_can; + +const char* SocketCanErrorToString(scpp::SocketCanStatus status) { + switch (status) { + case scpp::STATUS_OK: + return "No error"; + case scpp::STATUS_SOCKET_CREATE_ERROR: + return "SocketCAN socket creation error"; + case scpp::STATUS_INTERFACE_NAME_TO_IDX_ERROR: + return "SocketCAN interface name to index error"; + case scpp::STATUS_MTU_ERROR: + return "SocketCAN maximum transfer unit error"; + case scpp::STATUS_CANFD_NOT_SUPPORTED: + return "SocketCAN flexible data-rate not supported on this interface"; + case scpp::STATUS_ENABLE_FD_SUPPORT_ERROR: + return "Error enabling SocketCAN flexible-data-rate support"; + case scpp::STATUS_WRITE_ERROR: + return "SocketCAN write error"; + case scpp::STATUS_READ_ERROR: + return "SocketCAN read error"; + case scpp::STATUS_BIND_ERROR: + return "SocketCAN bind error"; + default: + return "Unknown SocketCAN error"; + } +} + +/** + * @brief Callback function to send a CAN frame. + * + * This function is called whenever a CAN frame needs to be transmitted. It sets up the necessary + * CAN frame fields and writes the frame using the SocketCAN interface. + * + * @param arbitration_id The frame arbitration id. + * @param data Pointer to the data array to be transmitted. + * @param data_length The size of transmitted data. + * @param rtr If the frame is of request transmit type (RTR). + * + * @throws std::runtime_error If the CAN frame write fails. + * + * @note The function logs debug messages about the CAN frame write status. + */ +void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_length, bool rtr) +{ + ROS_DEBUG_STREAM("Attempting to write CAN frame with arbitration_id: " << arbitration_id); + + scpp::CanFrame cf_to_write; + + cf_to_write.id = arbitration_id; + if (rtr) { + cf_to_write.id |= CAN_RTR_FLAG; // Set RTR flag if rtr argument + } + cf_to_write.len = data_length; + for (int i = 0; i < data_length; ++i) + cf_to_write.data[i] = data[i]; + auto write_status = socket_can.write(cf_to_write); + if (write_status != scpp::STATUS_OK) + { + throw std::runtime_error(SocketCanErrorToString(write_status)); + } + else + { + ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << arbitration_id << " written successfully."); + } +} + +/** + * @brief Callback function to receive a CAN frame. + * + * This function is called whenever a CAN frame is to be received. It attempts to read a CAN + * frame using the SocketCAN interface. + * + * @param arbitration_id Pointer to the frame arbitration id. + * @param data Pointer to the data array to be received. + * @param data_length Pointer to the variable that will hold the size of received data. + * + * @return True if the CAN frame was read successfully, false otherwise. + * + * @throws std::runtime_error If the CAN frame read fails. + * + * @note The function logs debug messages about the CAN frame read status. + */ +bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_length) +{ + ROS_DEBUG_STREAM("Attempting to read CAN frame..."); + + scpp::CanFrame fr; + scpp::SocketCanStatus read_status = socket_can.read(fr); + if (read_status == scpp::STATUS_OK) + { + *arbitration_id = fr.id & CAN_EFF_MASK; + *data_length = fr.len; + std::copy(fr.data, fr.data + fr.len, data); + ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << *arbitration_id << " read successfully."); + return true; + } + else + { + throw std::runtime_error(SocketCanErrorToString(read_status)); + return false; + } +} + +/** + * @brief Callback function to perform a delay. + * + * This function is called whenever a delay is required. It uses the ROS sleep functionality + * to create a delay for a specified duration in microseconds. + * + * @param us The microseconds to wait for. + * + * @note The function relies on the ROS sleep mechanism for precise timing. + */ +void delay_us_cb(uint32_t us) +{ + ros::Duration(us * 1e-6).sleep(); +} + +TinymovrJoint::TinymovrJoint() {} + +TinymovrJoint::~TinymovrJoint() {} + +/** + * @brief Initializes the TinymovrJoint instance. + * + * This function initializes the TinymovrJoint, setting up the servos based on the ROS parameters + * specified in the given NodeHandle. It also sets up the interfaces for joint state, position, + * velocity, and effort, and registers these interfaces. + * + * @param root_nh The root node handle. + * @param robot_hw_nh The robot hardware node handle. + * + * @return True if initialization succeeds, false otherwise. + * + * @note The initialization process involves several steps, including reading configuration + * from the ROS parameter server, setting up the SocketCAN interface, initializing each + * servo's mode and state, and registering the necessary hardware interfaces. + */ +bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) +{ + + XmlRpc::XmlRpcValue servos_param; + + bool got_all_params = true; + + // build servo instances from configuration + if (got_all_params &= robot_hw_nh.getParam("/tinymovr_joint_iface/joints", servos_param)) { + ROS_ASSERT(servos_param.getType() == XmlRpc::XmlRpcValue::TypeStruct); + try { + for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = servos_param.begin(); it != servos_param.end(); ++it) { + + std::string current_joint_name = static_cast(it->first); + ROS_INFO_STREAM("servo: " << current_joint_name); + joint_name.push_back(current_joint_name); // Store joint name + + id_t id; + int delay_us; + if (it->second.hasMember("id")) + { + id = static_cast(servos_param[it->first]["id"]); + delay_us = static_cast(servos_param[it->first]["delay_us"]); + ROS_INFO_STREAM("\tid: " << id << " delay_us: " << delay_us); + servos.push_back(Tinymovr(id, &send_cb, &recv_cb, &delay_us_cb, delay_us)); + servo_modes.push_back(servos_param[it->first]["command_interface"]); + rads_to_ticks.push_back(static_cast(servos_param[it->first]["rads_to_ticks"])); + } + else + { + ROS_ERROR_STREAM("servo " << it->first << " has no associated servo ID."); + continue; + } + } + } + catch (XmlRpc::XmlRpcException& e) + { + ROS_FATAL_STREAM("Exception raised by XmlRpc while reading the " + << "configuration: " << e.getMessage() << ".\n" + << "Please check the configuration, particularly parameter types."); + return false; + } + } + + if (!got_all_params) { + std::string sub_namespace = robot_hw_nh.getNamespace(); + std::string error_message = "One or more of the following parameters " + "were not set:\n" + + sub_namespace + "/tinymovr_joint_iface/joints"; + ROS_FATAL_STREAM(error_message); + return false; + } + + num_joints = servos_param.size(); + + joint_position_command.resize(num_joints, 0.0); + joint_velocity_command.resize(num_joints, 0.0); + joint_effort_command.resize(num_joints, 0.0); + joint_position_state.resize(num_joints, 0.0); + joint_velocity_state.resize(num_joints, 0.0); + joint_effort_state.resize(num_joints, 0.0); + + const scpp::SocketCanStatus status = socket_can.open("can0"); + if (scpp::STATUS_OK == status) + { + ROS_INFO("Socketcan opened successfully"); + } + else + { + ROS_ERROR("Cant' open Socketcan: %d", status); + exit(1); + } + + // initialize servos with correct mode + ROS_INFO("Asserting spec compatibility"); + for (int i=0; i +#include +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "tinymovr_joint_iface"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::NodeHandle nh; + + tinymovr_ros::TinymovrJoint tm_joint; + + bool init_success = tm_joint.init(nh, nh); + + controller_manager::ControllerManager cm(&tm_joint, nh); + + ros::Rate rate(100); // 100Hz update rate + + ROS_INFO("tinymovr joint interface started"); + while (ros::ok()) + { + tm_joint.read(ros::Time::now(), rate.expectedCycleTime()); + cm.update(ros::Time::now(), rate.expectedCycleTime()); + tm_joint.write(ros::Time::now(), rate.expectedCycleTime()); + rate.sleep(); + } + + tm_joint.shutdown(); + + spinner.stop(); + return 0; +} diff --git a/urdf/diffbot.urdf b/urdf/diffbot.urdf deleted file mode 100644 index 6d565d2..0000000 --- a/urdf/diffbot.urdf +++ /dev/null @@ -1,105 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file