From 900dedaa84c31c2ff530f7625fac840bda23a940 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 26 Jan 2023 16:38:03 +0200 Subject: [PATCH 01/61] remove diffbot files --- config/diffbot_hw_iface.yaml | 0 include/tinymovr_ros/diffbot_hw_iface.hpp | 38 ----- include/tinymovr_ros/tinymovr_can.hpp | 21 --- include/tinymovr_ros/tm_hw_iface.hpp | 53 ------- rviz/diffbot.rviz | 174 ---------------------- src/diffbot_hw_iface.cpp | 74 --------- src/diffbot_hw_iface_node.cpp | 31 ---- urdf/diffbot.urdf | 105 ------------- 8 files changed, 496 deletions(-) delete mode 100644 config/diffbot_hw_iface.yaml delete mode 100644 include/tinymovr_ros/diffbot_hw_iface.hpp delete mode 100644 include/tinymovr_ros/tinymovr_can.hpp delete mode 100644 include/tinymovr_ros/tm_hw_iface.hpp delete mode 100644 rviz/diffbot.rviz delete mode 100644 src/diffbot_hw_iface.cpp delete mode 100644 src/diffbot_hw_iface_node.cpp delete mode 100644 urdf/diffbot.urdf diff --git a/config/diffbot_hw_iface.yaml b/config/diffbot_hw_iface.yaml deleted file mode 100644 index e69de29..0000000 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/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/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 From 991ab7a4acfb4abffc7f57f70f944374b35565f8 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 26 Jan 2023 16:39:33 +0200 Subject: [PATCH 02/61] add avlos generated headers --- include/tinymovr/can.hpp | 24 ++++ include/tinymovr/comms.hpp | 23 ++++ include/tinymovr/controller.hpp | 44 +++++++ include/tinymovr/current.hpp | 33 +++++ include/tinymovr/encoder.hpp | 28 +++++ include/tinymovr/helpers.hpp | 195 ++++++++++++++++++++++++++++++ include/tinymovr/motor.hpp | 36 ++++++ include/tinymovr/position.hpp | 24 ++++ include/tinymovr/scheduler.hpp | 23 ++++ include/tinymovr/tinymovr.hpp | 103 ++++++++++++++++ include/tinymovr/traj_planner.hpp | 29 +++++ include/tinymovr/velocity.hpp | 32 +++++ include/tinymovr/voltage.hpp | 21 ++++ include/tinymovr/watchdog.hpp | 25 ++++ include/tinymovr_can.hpp | 22 ++++ include/tm_hw_iface.hpp | 69 +++++++++++ src/tinymovr/can.cpp | 46 +++++++ src/tinymovr/comms.cpp | 12 ++ src/tinymovr/controller.cpp | 104 ++++++++++++++++ src/tinymovr/current.cpp | 130 ++++++++++++++++++++ src/tinymovr/encoder.cpp | 90 ++++++++++++++ src/tinymovr/motor.cpp | 153 +++++++++++++++++++++++ src/tinymovr/position.cpp | 46 +++++++ src/tinymovr/scheduler.cpp | 45 +++++++ src/tinymovr/tinymovr.cpp | 104 ++++++++++++++++ src/tinymovr/traj_planner.cpp | 92 ++++++++++++++ src/tinymovr/velocity.cpp | 114 +++++++++++++++++ src/tinymovr/voltage.cpp | 23 ++++ src/tinymovr/watchdog.cpp | 57 +++++++++ src/tinymovr_can.cpp | 9 +- src/tm_hw_iface.cpp | 43 ++++++- src/tm_hw_iface_node.cpp | 18 +-- 32 files changed, 1803 insertions(+), 14 deletions(-) create mode 100644 include/tinymovr/can.hpp create mode 100644 include/tinymovr/comms.hpp create mode 100644 include/tinymovr/controller.hpp create mode 100644 include/tinymovr/current.hpp create mode 100644 include/tinymovr/encoder.hpp create mode 100644 include/tinymovr/helpers.hpp create mode 100644 include/tinymovr/motor.hpp create mode 100644 include/tinymovr/position.hpp create mode 100644 include/tinymovr/scheduler.hpp create mode 100644 include/tinymovr/tinymovr.hpp create mode 100644 include/tinymovr/traj_planner.hpp create mode 100644 include/tinymovr/velocity.hpp create mode 100644 include/tinymovr/voltage.hpp create mode 100644 include/tinymovr/watchdog.hpp create mode 100644 include/tinymovr_can.hpp create mode 100644 include/tm_hw_iface.hpp create mode 100644 src/tinymovr/can.cpp create mode 100644 src/tinymovr/comms.cpp create mode 100644 src/tinymovr/controller.cpp create mode 100644 src/tinymovr/current.cpp create mode 100644 src/tinymovr/encoder.cpp create mode 100644 src/tinymovr/motor.cpp create mode 100644 src/tinymovr/position.cpp create mode 100644 src/tinymovr/scheduler.cpp create mode 100644 src/tinymovr/tinymovr.cpp create mode 100644 src/tinymovr/traj_planner.cpp create mode 100644 src/tinymovr/velocity.cpp create mode 100644 src/tinymovr/voltage.cpp create mode 100644 src/tinymovr/watchdog.cpp diff --git a/include/tinymovr/can.hpp b/include/tinymovr/can.hpp new file mode 100644 index 0000000..4dd3a32 --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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..4db122a --- /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): + Node(_can_node_id, _send_cb, _recv_cb) + , can(_can_node_id, _send_cb, _recv_cb) {}; + Can_ can; + +}; diff --git a/include/tinymovr/controller.hpp b/include/tinymovr/controller.hpp new file mode 100644 index 0000000..bb410b8 --- /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): + Node(_can_node_id, _send_cb, _recv_cb) + , position(_can_node_id, _send_cb, _recv_cb) + , velocity(_can_node_id, _send_cb, _recv_cb) + , current(_can_node_id, _send_cb, _recv_cb) + , voltage(_can_node_id, _send_cb, _recv_cb) {}; + 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..2fe667f --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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..edc79fe --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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..10a5200 --- /dev/null +++ b/include/tinymovr/helpers.hpp @@ -0,0 +1,195 @@ +/* +* 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 +#endif + +#if defined ARDUINO +#include "Arduino.h" +#endif + +#define EP_BITS (6) +#define RECV_DELAY_US (160.0f) + +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); + +class Node { + public: + + Node(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): + can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb) {} + + protected: + uint8_t can_node_id; + send_callback send_cb; + recv_callback recv_cb; + uint8_t _data[8]; + uint8_t _dlc; + uint8_t get_arbitration_id(uint8_t cmd_id) { + return this->can_node_id << EP_BITS | cmd_id; + } + void send(uint8_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr) + { + const uint8_t arb_id = this->get_arbitration_id(cmd_id); + this->send_cb(arb_id, data, data_size, rtr); + } + + bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us) + { + // 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) + { + delayMicroseconds(delay_us); + } + const uint8_t arb_id = this->get_arbitration_id(cmd_id); + return this->recv_cb(arb_id, data, data_size); + } +}; + +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(uint16_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(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(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) { + //static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + //static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + 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/motor.hpp b/include/tinymovr/motor.hpp new file mode 100644 index 0000000..fff17c0 --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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..451afe9 --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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..a1d9bd5 --- /dev/null +++ b/include/tinymovr/scheduler.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 + +class Scheduler_ : Node +{ + public: + + Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): + Node(_can_node_id, _send_cb, _recv_cb) {}; + uint32_t get_total(void); + uint32_t get_busy(void); + uint8_t get_errors(void); + +}; diff --git a/include/tinymovr/tinymovr.hpp b/include/tinymovr/tinymovr.hpp new file mode 100644 index 0000000..1b09900 --- /dev/null +++ b/include/tinymovr/tinymovr.hpp @@ -0,0 +1,103 @@ +/* +* 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 + +static uint32_t avlos_proto_hash = 3273002564; + +enum errors_flags +{ + ERRORS_NONE = 0, + ERRORS_UNDERVOLTAGE = (1 << 0), + ERRORS_DRIVER_FAULT = (1 << 1) +}; + +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) +}; + +class Tinymovr : Node +{ + public: + + Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): + Node(_can_node_id, _send_cb, _recv_cb) + , scheduler(_can_node_id, _send_cb, _recv_cb) + , controller(_can_node_id, _send_cb, _recv_cb) + , comms(_can_node_id, _send_cb, _recv_cb) + , motor(_can_node_id, _send_cb, _recv_cb) + , encoder(_can_node_id, _send_cb, _recv_cb) + , traj_planner(_can_node_id, _send_cb, _recv_cb) + , watchdog(_can_node_id, _send_cb, _recv_cb) {}; + uint32_t get_protocol_hash(void); + uint32_t get_uid(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(); + Scheduler_ scheduler; + Controller_ controller; + Comms_ comms; + Motor_ motor; + Encoder_ encoder; + Traj_planner_ traj_planner; + Watchdog_ watchdog; + +}; diff --git a/include/tinymovr/traj_planner.hpp b/include/tinymovr/traj_planner.hpp new file mode 100644 index 0000000..0901cbf --- /dev/null +++ b/include/tinymovr/traj_planner.hpp @@ -0,0 +1,29 @@ +/* +* 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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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); + 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..a8525ff --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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..ed19750 --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + float get_Vq_setpoint(void); + +}; diff --git a/include/tinymovr/watchdog.hpp b/include/tinymovr/watchdog.hpp new file mode 100644 index 0000000..19d8180 --- /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): + Node(_can_node_id, _send_cb, _recv_cb) {}; + 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_can.hpp b/include/tinymovr_can.hpp new file mode 100644 index 0000000..240d547 --- /dev/null +++ b/include/tinymovr_can.hpp @@ -0,0 +1,22 @@ + +#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); + bool write_frame(uint32_t arbitration_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/tm_hw_iface.hpp b/include/tm_hw_iface.hpp new file mode 100644 index 0000000..c30b236 --- /dev/null +++ b/include/tm_hw_iface.hpp @@ -0,0 +1,69 @@ + +#pragma once + +#include +#include +#include +#include +#include + +namespace tinymovr_ros +{ + +// --------------------------------------------------------------- +/* + * Function: send_cb + * -------------------- + * Is called to send a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be transmitted + * data_size: the size of transmitted data + * rtr: if the ftame is of request transmit type (RTR) + */ +void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +{ + return tmcan.write_frame(arbitration_id, data, data_size) +} + +/* + * Function: recv_cb + * -------------------- + * Is called to receive a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be received + * data_size: pointer to the variable that will hold the size of received data + */ +bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) +{ + (void)arbitration_id; + return tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_size); +} +// --------------------------------------------------------------- + + +class TinymovrHW : public hardware_interface::RobotHW +{ +public: + TinymovrHW(); + 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; + uint8_t node_id; + + Tinymovr tm; +}; + +} \ No newline at end of file diff --git a/src/tinymovr/can.cpp b/src/tinymovr/can.cpp new file mode 100644 index 0000000..028d78d --- /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(41, this->_data, 0, true); + if (this->recv(41, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Can_::set_rate(uint32_t value) +{ + write_le(value, this->_data); + this->send(41, this->_data, sizeof(uint32_t), false); +} + +uint32_t Can_::get_id(void) +{ + uint32_t value = 0; + this->send(42, this->_data, 0, true); + if (this->recv(42, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Can_::set_id(uint32_t value) +{ + write_le(value, this->_data); + this->send(42, 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..3254f19 --- /dev/null +++ b/src/tinymovr/controller.cpp @@ -0,0 +1,104 @@ +/* +* 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(14, this->_data, 0, true); + if (this->recv(14, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Controller_::set_state(uint8_t value) +{ + write_le(value, this->_data); + this->send(14, this->_data, sizeof(uint8_t), false); +} + +uint8_t Controller_::get_mode(void) +{ + uint8_t value = 0; + this->send(15, this->_data, 0, true); + if (this->recv(15, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Controller_::set_mode(uint8_t value) +{ + write_le(value, this->_data); + this->send(15, this->_data, sizeof(uint8_t), false); +} + +uint8_t Controller_::get_warnings(void) +{ + uint8_t value = 0; + this->send(16, this->_data, 0, true); + if (this->recv(16, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +uint8_t Controller_::get_errors(void) +{ + uint8_t value = 0; + this->send(17, this->_data, 0, true); + if (this->recv(17, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + + +void Controller_::calibrate() +{ + this->send(35, this->_data, 0, true); +} + +void Controller_::idle() +{ + this->send(36, this->_data, 0, true); +} + +void Controller_::position_mode() +{ + this->send(37, this->_data, 0, true); +} + +void Controller_::velocity_mode() +{ + this->send(38, this->_data, 0, true); +} + +void Controller_::current_mode() +{ + this->send(39, 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(40, this->_data, data_len, false); +} + + diff --git a/src/tinymovr/current.cpp b/src/tinymovr/current.cpp new file mode 100644 index 0000000..3a72f4b --- /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(26, this->_data, 0, true); + if (this->recv(26, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_Iq_setpoint(float value) +{ + write_le(value, this->_data); + this->send(26, this->_data, sizeof(float), false); +} + +float Current_::get_Id_setpoint(void) +{ + float value = 0; + this->send(27, this->_data, 0, true); + if (this->recv(27, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +float Current_::get_Iq_limit(void) +{ + float value = 0; + this->send(28, this->_data, 0, true); + if (this->recv(28, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_Iq_limit(float value) +{ + write_le(value, this->_data); + this->send(28, this->_data, sizeof(float), false); +} + +float Current_::get_Iq_estimate(void) +{ + float value = 0; + this->send(29, this->_data, 0, true); + if (this->recv(29, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +float Current_::get_bandwidth(void) +{ + float value = 0; + this->send(30, this->_data, 0, true); + if (this->recv(30, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_bandwidth(float value) +{ + write_le(value, this->_data); + this->send(30, this->_data, sizeof(float), false); +} + +float Current_::get_Iq_p_gain(void) +{ + float value = 0; + this->send(31, this->_data, 0, true); + if (this->recv(31, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +float Current_::get_max_Ibus_regen(void) +{ + float value = 0; + this->send(32, this->_data, 0, true); + if (this->recv(32, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_max_Ibus_regen(float value) +{ + write_le(value, this->_data); + this->send(32, this->_data, sizeof(float), false); +} + +float Current_::get_max_Ibrake(void) +{ + float value = 0; + this->send(33, this->_data, 0, true); + if (this->recv(33, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Current_::set_max_Ibrake(float value) +{ + write_le(value, this->_data); + this->send(33, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr/encoder.cpp b/src/tinymovr/encoder.cpp new file mode 100644 index 0000000..d919bf8 --- /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(52, this->_data, 0, true); + if (this->recv(52, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +float Encoder_::get_velocity_estimate(void) +{ + float value = 0; + this->send(53, this->_data, 0, true); + if (this->recv(53, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +uint8_t Encoder_::get_type(void) +{ + uint8_t value = 0; + this->send(54, this->_data, 0, true); + if (this->recv(54, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Encoder_::set_type(uint8_t value) +{ + write_le(value, this->_data); + this->send(54, this->_data, sizeof(uint8_t), false); +} + +float Encoder_::get_bandwidth(void) +{ + float value = 0; + this->send(55, this->_data, 0, true); + if (this->recv(55, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Encoder_::set_bandwidth(float value) +{ + write_le(value, this->_data); + this->send(55, this->_data, sizeof(float), false); +} + +bool Encoder_::get_calibrated(void) +{ + bool value = 0; + this->send(56, this->_data, 0, true); + if (this->recv(56, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +uint8_t Encoder_::get_errors(void) +{ + uint8_t value = 0; + this->send(57, this->_data, 0, true); + if (this->recv(57, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + + + diff --git a/src/tinymovr/motor.cpp b/src/tinymovr/motor.cpp new file mode 100644 index 0000000..db9ff2d --- /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(43, this->_data, 0, true); + if (this->recv(43, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_R(float value) +{ + write_le(value, this->_data); + this->send(43, this->_data, sizeof(float), false); +} + +float Motor_::get_L(void) +{ + float value = 0; + this->send(44, this->_data, 0, true); + if (this->recv(44, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_L(float value) +{ + write_le(value, this->_data); + this->send(44, this->_data, sizeof(float), false); +} + +uint8_t Motor_::get_pole_pairs(void) +{ + uint8_t value = 0; + this->send(45, this->_data, 0, true); + if (this->recv(45, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_pole_pairs(uint8_t value) +{ + write_le(value, this->_data); + this->send(45, this->_data, sizeof(uint8_t), false); +} + +uint8_t Motor_::get_type(void) +{ + uint8_t value = 0; + this->send(46, this->_data, 0, true); + if (this->recv(46, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_type(uint8_t value) +{ + write_le(value, this->_data); + this->send(46, this->_data, sizeof(uint8_t), false); +} + +float Motor_::get_offset(void) +{ + float value = 0; + this->send(47, this->_data, 0, true); + if (this->recv(47, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_offset(float value) +{ + write_le(value, this->_data); + this->send(47, this->_data, sizeof(float), false); +} + +int8_t Motor_::get_direction(void) +{ + int8_t value = 0; + this->send(48, this->_data, 0, true); + if (this->recv(48, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_direction(int8_t value) +{ + write_le(value, this->_data); + this->send(48, this->_data, sizeof(int8_t), false); +} + +bool Motor_::get_calibrated(void) +{ + bool value = 0; + this->send(49, this->_data, 0, true); + if (this->recv(49, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +float Motor_::get_I_cal(void) +{ + float value = 0; + this->send(50, this->_data, 0, true); + if (this->recv(50, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Motor_::set_I_cal(float value) +{ + write_le(value, this->_data); + this->send(50, this->_data, sizeof(float), false); +} + +uint8_t Motor_::get_errors(void) +{ + uint8_t value = 0; + this->send(51, this->_data, 0, true); + if (this->recv(51, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + 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..4f52c52 --- /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(18, this->_data, 0, true); + if (this->recv(18, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Position_::set_setpoint(float value) +{ + write_le(value, this->_data); + this->send(18, this->_data, sizeof(float), false); +} + +float Position_::get_p_gain(void) +{ + float value = 0; + this->send(19, this->_data, 0, true); + if (this->recv(19, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Position_::set_p_gain(float value) +{ + write_le(value, this->_data); + this->send(19, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr/scheduler.cpp b/src/tinymovr/scheduler.cpp new file mode 100644 index 0000000..07be947 --- /dev/null +++ b/src/tinymovr/scheduler.cpp @@ -0,0 +1,45 @@ +/* +* 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 Scheduler_::get_total(void) +{ + uint32_t value = 0; + this->send(11, this->_data, 0, true); + if (this->recv(11, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +uint32_t Scheduler_::get_busy(void) +{ + uint32_t value = 0; + this->send(12, this->_data, 0, true); + if (this->recv(12, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +uint8_t Scheduler_::get_errors(void) +{ + uint8_t value = 0; + this->send(13, this->_data, 0, true); + if (this->recv(13, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + + + diff --git a/src/tinymovr/tinymovr.cpp b/src/tinymovr/tinymovr.cpp new file mode 100644 index 0000000..82fe78e --- /dev/null +++ b/src/tinymovr/tinymovr.cpp @@ -0,0 +1,104 @@ +/* +* 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), RECV_DELAY_US)) + { + 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), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_Vbus(void) +{ + float value = 0; + this->send(2, this->_data, 0, true); + if (this->recv(2, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_Ibus(void) +{ + float value = 0; + this->send(3, this->_data, 0, true); + if (this->recv(3, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_power(void) +{ + float value = 0; + this->send(4, this->_data, 0, true); + if (this->recv(4, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} +float Tinymovr::get_temp(void) +{ + float value = 0; + this->send(5, this->_data, 0, true); + if (this->recv(5, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} +bool Tinymovr::get_calibrated(void) +{ + bool value = 0; + this->send(6, this->_data, 0, true); + if (this->recv(6, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} +uint8_t Tinymovr::get_errors(void) +{ + uint8_t value = 0; + this->send(7, this->_data, 0, true); + if (this->recv(7, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Tinymovr::save_config() +{ + this->send(8, this->_data, 0, true); +} + +void Tinymovr::erase_config() +{ + this->send(9, this->_data, 0, true); +} + +void Tinymovr::reset() +{ + this->send(10, this->_data, 0, true); +} + diff --git a/src/tinymovr/traj_planner.cpp b/src/tinymovr/traj_planner.cpp new file mode 100644 index 0000000..604a48d --- /dev/null +++ b/src/tinymovr/traj_planner.cpp @@ -0,0 +1,92 @@ +/* +* 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(58, this->_data, 0, true); + if (this->recv(58, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_max_accel(float value) +{ + write_le(value, this->_data); + this->send(58, this->_data, sizeof(float), false); +} + +float Traj_planner_::get_max_decel(void) +{ + float value = 0; + this->send(59, this->_data, 0, true); + if (this->recv(59, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_max_decel(float value) +{ + write_le(value, this->_data); + this->send(59, this->_data, sizeof(float), false); +} + +float Traj_planner_::get_max_vel(void) +{ + float value = 0; + this->send(60, this->_data, 0, true); + if (this->recv(60, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Traj_planner_::set_max_vel(float value) +{ + write_le(value, this->_data); + this->send(60, 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(61, 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(62, this->_data, data_len, false); +} +uint8_t Traj_planner_::get_errors(void) +{ + uint8_t value = 0; + this->send(63, this->_data, 0, true); + if (this->recv(63, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + 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..901b6fe --- /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(20, this->_data, 0, true); + if (this->recv(20, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_setpoint(float value) +{ + write_le(value, this->_data); + this->send(20, this->_data, sizeof(float), false); +} + +float Velocity_::get_limit(void) +{ + float value = 0; + this->send(21, this->_data, 0, true); + if (this->recv(21, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_limit(float value) +{ + write_le(value, this->_data); + this->send(21, this->_data, sizeof(float), false); +} + +float Velocity_::get_p_gain(void) +{ + float value = 0; + this->send(22, this->_data, 0, true); + if (this->recv(22, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_p_gain(float value) +{ + write_le(value, this->_data); + this->send(22, this->_data, sizeof(float), false); +} + +float Velocity_::get_i_gain(void) +{ + float value = 0; + this->send(23, this->_data, 0, true); + if (this->recv(23, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_i_gain(float value) +{ + write_le(value, this->_data); + this->send(23, this->_data, sizeof(float), false); +} + +float Velocity_::get_deadband(void) +{ + float value = 0; + this->send(24, this->_data, 0, true); + if (this->recv(24, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_deadband(float value) +{ + write_le(value, this->_data); + this->send(24, this->_data, sizeof(float), false); +} + +float Velocity_::get_increment(void) +{ + float value = 0; + this->send(25, this->_data, 0, true); + if (this->recv(25, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Velocity_::set_increment(float value) +{ + write_le(value, this->_data); + this->send(25, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr/voltage.cpp b/src/tinymovr/voltage.cpp new file mode 100644 index 0000000..151fb16 --- /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(34, this->_data, 0, true); + if (this->recv(34, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + 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..f400346 --- /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(64, this->_data, 0, true); + if (this->recv(64, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Watchdog_::set_enabled(bool value) +{ + write_le(value, this->_data); + this->send(64, this->_data, sizeof(bool), false); +} + +bool Watchdog_::get_triggered(void) +{ + bool value = 0; + this->send(65, this->_data, 0, true); + if (this->recv(65, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +float Watchdog_::get_timeout(void) +{ + float value = 0; + this->send(66, this->_data, 0, true); + if (this->recv(66, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; +} + +void Watchdog_::set_timeout(float value) +{ + write_le(value, this->_data); + this->send(66, this->_data, sizeof(float), false); +} + + + diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp index b186bb6..be7e18d 100644 --- a/src/tinymovr_can.cpp +++ b/src/tinymovr_can.cpp @@ -1,5 +1,5 @@ -#include +#include namespace tinymovr_ros { @@ -39,10 +39,15 @@ bool TinymovrCAN::read_frame(uint32_t node_id, uint32_t ep_id, uint8_t* data, ui } bool TinymovrCAN::write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len) +{ + return write_frame(make_arbitration_id(node_id, ep_id)); +} + +bool TinymovrCAN::write_frame(uint32_t arbitration_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.id = arbitration_id; cf_to_write.len = data_len; for (int i = 0; i < data_len; ++i) cf_to_write.data[i] = data[i]; diff --git a/src/tm_hw_iface.cpp b/src/tm_hw_iface.cpp index 2919d0f..5363809 100644 --- a/src/tm_hw_iface.cpp +++ b/src/tm_hw_iface.cpp @@ -1,14 +1,51 @@ -#include +#include +#include +#include + +#include +#include +#include using namespace tinymovr_ros; -bool Tinymovr::read(const ros::Duration& dt) +TinymovrHW::TinymovrHW(): tm(1, send_cb, recv_cb) +{ + tmcan.init(); + + // connect and register the joint state interface + hardware_interface::JointStateHandle state_handle_a("Actuator", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); + jnt_state_interface.registerHandle(state_handle_a); + registerInterface(&jnt_state_interface); + + // connect and register the joint position interface + hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[0]); + jnt_pos_interface.registerHandle(pos_handle_a); + registerInterface(&jnt_pos_interface); + + // connect and register the joint velocity interface + hardware_interface::JointHandle vel_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[1]); + jnt_vel_interface.registerHandle(vel_handle_a); + registerInterface(&jnt_vel_interface); + + // connect and register the joint effort interface + hardware_interface::JointHandle eff_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[2]); + jnt_eff_interface.registerHandle(eff_handle_a); + registerInterface(&jnt_eff_interface); +} + +bool TinymovrHW::read(const ros::Duration& dt) { + pos = tm.encoder.get_position_estimate(); + vel = tm.encoder.get_velocity_estimate(); + eff = tm.controller.current.get_Iq_estimate(); return true; } -bool Tinymovr::write() +bool TinymovrHW::write() { + tm.controller.position.set_setpoint(cmd_pos); + tm.controller.velocity.set_setpoint(cmd_vel); + tm.controller.current.set_Iq_setpoint(cmd_eff); return true; } \ No newline at end of file diff --git a/src/tm_hw_iface_node.cpp b/src/tm_hw_iface_node.cpp index 6cba2f4..8d4b4fc 100644 --- a/src/tm_hw_iface_node.cpp +++ b/src/tm_hw_iface_node.cpp @@ -1,30 +1,30 @@ -#include +#include #include int main(int argc, char **argv) { ros::init(argc, argv, "tinymovr_ros_iface_node"); - tinymovr_ros::Tinymovr tm; + tinymovr_ros::TinymovrHW tm_hw; ros::NodeHandle nh; controller_manager::ControllerManager cm(&tm, nh); - ros::Time last, now; - now = last = ros::Time::now(); - ros::Duration period(0.05); + ros::Rate rate(20.0); + ros::Time last; ros::AsyncSpinner spinner(2); spinner.start(); while (ros::ok()) { - now = ros::Time::now(); - period = now - last; + const ros::Time now = ros::Time::now(); + const ros::Duration period = now - last; last = now; - tm.read(period); + tm_hw.read(period); cm.update(now, period); - tm.write(); + tm_hw.write(); + rate.sleep(); } spinner.stop(); return 0; From 5d02c5ef72ad74aa8f1ab36a303dcf4465c67b1c Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 26 Jan 2023 16:39:40 +0200 Subject: [PATCH 03/61] add avlos config file --- avlos_config.yaml | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 avlos_config.yaml 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 + From c0df4ec616bf57ab70baab02618d6900f21ad56b Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Mon, 30 Jan 2023 16:52:20 +0200 Subject: [PATCH 04/61] implement initial actuator iface --- include/tm_actuator_iface.hpp | 69 +++++++++++++++++++ .../{tm_hw_iface.hpp => tm_joint_iface.hpp} | 4 +- src/tm_actuator_iface.cpp | 51 ++++++++++++++ src/{tm_hw_iface.cpp => tm_joint_iface.cpp} | 8 +-- ...iface_node.cpp => tm_joint_iface_node.cpp} | 10 +-- 5 files changed, 131 insertions(+), 11 deletions(-) create mode 100644 include/tm_actuator_iface.hpp rename include/{tm_hw_iface.hpp => tm_joint_iface.hpp} (95%) create mode 100644 src/tm_actuator_iface.cpp rename src/{tm_hw_iface.cpp => tm_joint_iface.cpp} (90%) rename src/{tm_hw_iface_node.cpp => tm_joint_iface_node.cpp} (73%) diff --git a/include/tm_actuator_iface.hpp b/include/tm_actuator_iface.hpp new file mode 100644 index 0000000..1b4b47a --- /dev/null +++ b/include/tm_actuator_iface.hpp @@ -0,0 +1,69 @@ + +#pragma once + +#include +#include +#include +#include +#include + +namespace tinymovr_ros +{ + +// --------------------------------------------------------------- +/* + * Function: send_cb + * -------------------- + * Is called to send a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be transmitted + * data_size: the size of transmitted data + * rtr: if the ftame is of request transmit type (RTR) + */ +void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +{ + return tmcan.write_frame(arbitration_id, data, data_size) +} + +/* + * Function: recv_cb + * -------------------- + * Is called to receive a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be received + * data_size: pointer to the variable that will hold the size of received data + */ +bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) +{ + (void)arbitration_id; + return tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_size); +} +// --------------------------------------------------------------- + + +class TinymovrActuator : public hardware_interface::RobotHW +{ +public: + TinymovrActuator(); + 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; + uint8_t node_id; + + Tinymovr tm; +}; + +} \ No newline at end of file diff --git a/include/tm_hw_iface.hpp b/include/tm_joint_iface.hpp similarity index 95% rename from include/tm_hw_iface.hpp rename to include/tm_joint_iface.hpp index c30b236..db09628 100644 --- a/include/tm_hw_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -43,10 +43,10 @@ bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) // --------------------------------------------------------------- -class TinymovrHW : public hardware_interface::RobotHW +class TinymovrJoint : public hardware_interface::RobotHW { public: - TinymovrHW(); + TinymovrJoint(); bool read(const ros::Duration& period); bool write(); diff --git a/src/tm_actuator_iface.cpp b/src/tm_actuator_iface.cpp new file mode 100644 index 0000000..ee52d5b --- /dev/null +++ b/src/tm_actuator_iface.cpp @@ -0,0 +1,51 @@ + +#include +#include +#include + +#include +#include +#include + +using namespace tinymovr_ros; + +TinymovrHW::TinymovrActuator(): tm(1, send_cb, recv_cb) +{ + tmcan.init(); + + // connect and register the joint state interface + hardware_interface::JointStateHandle state_handle_a("Actuator", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); + jnt_state_interface.registerHandle(state_handle_a); + registerInterface(&jnt_state_interface); + + // connect and register the joint position interface + hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[0]); + jnt_pos_interface.registerHandle(pos_handle_a); + registerInterface(&jnt_pos_interface); + + // connect and register the joint velocity interface + hardware_interface::JointHandle vel_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[1]); + jnt_vel_interface.registerHandle(vel_handle_a); + registerInterface(&jnt_vel_interface); + + // connect and register the joint effort interface + hardware_interface::JointHandle eff_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[2]); + jnt_eff_interface.registerHandle(eff_handle_a); + registerInterface(&jnt_eff_interface); +} + +bool TinymovrActuator::read(const ros::Duration& dt) +{ + pos = tm.encoder.get_position_estimate(); + vel = tm.encoder.get_velocity_estimate(); + eff = tm.controller.current.get_Iq_estimate(); + return true; +} + +bool TinymovrActuator::write() +{ + tm.controller.position.set_setpoint(cmd_pos); + tm.controller.velocity.set_setpoint(cmd_vel); + tm.controller.current.set_Iq_setpoint(cmd_eff); + return true; +} \ No newline at end of file diff --git a/src/tm_hw_iface.cpp b/src/tm_joint_iface.cpp similarity index 90% rename from src/tm_hw_iface.cpp rename to src/tm_joint_iface.cpp index 5363809..af4b955 100644 --- a/src/tm_hw_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -5,11 +5,11 @@ #include #include -#include +#include using namespace tinymovr_ros; -TinymovrHW::TinymovrHW(): tm(1, send_cb, recv_cb) +TinymovrHW::TinymovrJoint(): tm(1, send_cb, recv_cb) { tmcan.init(); @@ -34,7 +34,7 @@ TinymovrHW::TinymovrHW(): tm(1, send_cb, recv_cb) registerInterface(&jnt_eff_interface); } -bool TinymovrHW::read(const ros::Duration& dt) +bool TinymovrJoint::read(const ros::Duration& dt) { pos = tm.encoder.get_position_estimate(); vel = tm.encoder.get_velocity_estimate(); @@ -42,7 +42,7 @@ bool TinymovrHW::read(const ros::Duration& dt) return true; } -bool TinymovrHW::write() +bool TinymovrJoint::write() { tm.controller.position.set_setpoint(cmd_pos); tm.controller.velocity.set_setpoint(cmd_vel); diff --git a/src/tm_hw_iface_node.cpp b/src/tm_joint_iface_node.cpp similarity index 73% rename from src/tm_hw_iface_node.cpp rename to src/tm_joint_iface_node.cpp index 8d4b4fc..bde7ef2 100644 --- a/src/tm_hw_iface_node.cpp +++ b/src/tm_joint_iface_node.cpp @@ -1,12 +1,12 @@ -#include +#include #include int main(int argc, char **argv) { - ros::init(argc, argv, "tinymovr_ros_iface_node"); + ros::init(argc, argv, "tinymovr_joint_iface_node"); - tinymovr_ros::TinymovrHW tm_hw; + tinymovr_ros::TinymovrJoint tm_joint; ros::NodeHandle nh; controller_manager::ControllerManager cm(&tm, nh); @@ -21,9 +21,9 @@ int main(int argc, char **argv) const ros::Time now = ros::Time::now(); const ros::Duration period = now - last; last = now; - tm_hw.read(period); + tm_joint.read(period); cm.update(now, period); - tm_hw.write(); + tm_joint.write(); rate.sleep(); } spinner.stop(); From 47785e4f4180c1b3af10af925772e9997cde7635 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Mon, 30 Jan 2023 17:01:13 +0200 Subject: [PATCH 05/61] refactor for actuator iface --- ...m_hw_iface.yaml => tm_actuator_iface.yaml} | 0 config/tm_joint_iface.yaml | 0 include/tm_actuator_iface.hpp | 12 +++--- launch/diffbot_ros_node.launch | 18 --------- launch/tinymovr_actuator_iface_node.launch | 5 +++ launch/tinymovr_joint_iface_node.launch | 5 +++ launch/tinymovr_ros_node.launch | 5 --- src/tm_actuator_iface.cpp | 38 +++++++++---------- src/tm_joint_iface.cpp | 8 ++-- 9 files changed, 39 insertions(+), 52 deletions(-) rename config/{tm_hw_iface.yaml => tm_actuator_iface.yaml} (100%) create mode 100644 config/tm_joint_iface.yaml delete mode 100644 launch/diffbot_ros_node.launch create mode 100644 launch/tinymovr_actuator_iface_node.launch create mode 100644 launch/tinymovr_joint_iface_node.launch delete mode 100644 launch/tinymovr_ros_node.launch diff --git a/config/tm_hw_iface.yaml b/config/tm_actuator_iface.yaml similarity index 100% rename from config/tm_hw_iface.yaml rename to config/tm_actuator_iface.yaml diff --git a/config/tm_joint_iface.yaml b/config/tm_joint_iface.yaml new file mode 100644 index 0000000..e69de29 diff --git a/include/tm_actuator_iface.hpp b/include/tm_actuator_iface.hpp index 1b4b47a..a656581 100644 --- a/include/tm_actuator_iface.hpp +++ b/include/tm_actuator_iface.hpp @@ -1,8 +1,8 @@ #pragma once -#include -#include +#include +#include #include #include #include @@ -51,10 +51,10 @@ class TinymovrActuator : public hardware_interface::RobotHW 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; + hardware_interface::ActuatorStateInterface act_state_interface; + hardware_interface::PositionActuatorInterface act_pos_interface; + hardware_interface::VelocityActuatorInterface act_vel_interface; + hardware_interface::EffortActuatorInterface act_eff_interface; double cmd_pos; double cmd_vel; double cmd_eff; 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_actuator_iface_node.launch b/launch/tinymovr_actuator_iface_node.launch new file mode 100644 index 0000000..0163c75 --- /dev/null +++ b/launch/tinymovr_actuator_iface_node.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/launch/tinymovr_joint_iface_node.launch b/launch/tinymovr_joint_iface_node.launch new file mode 100644 index 0000000..452139c --- /dev/null +++ b/launch/tinymovr_joint_iface_node.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file 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/src/tm_actuator_iface.cpp b/src/tm_actuator_iface.cpp index ee52d5b..b8dc0dd 100644 --- a/src/tm_actuator_iface.cpp +++ b/src/tm_actuator_iface.cpp @@ -13,25 +13,25 @@ TinymovrHW::TinymovrActuator(): tm(1, send_cb, recv_cb) { tmcan.init(); - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_a("Actuator", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); - jnt_state_interface.registerHandle(state_handle_a); - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[0]); - jnt_pos_interface.registerHandle(pos_handle_a); - registerInterface(&jnt_pos_interface); - - // connect and register the joint velocity interface - hardware_interface::JointHandle vel_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[1]); - jnt_vel_interface.registerHandle(vel_handle_a); - registerInterface(&jnt_vel_interface); - - // connect and register the joint effort interface - hardware_interface::JointHandle eff_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[2]); - jnt_eff_interface.registerHandle(eff_handle_a); - registerInterface(&jnt_eff_interface); + // connect and register the actuator state interface + hardware_interface::ActuatorStateHandle state_handle_a("Actuator", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); + act_state_interface.registerHandle(state_handle_a); + registerInterface(&act_state_interface); + + // connect and register the actuator position interface + hardware_interface::ActuatorHandle pos_handle_a(act_state_interface.getHandle("Actuator"), &hw_commands_[0]); + act_pos_interface.registerHandle(pos_handle_a); + registerInterface(&act_pos_interface); + + // connect and register the actuator velocity interface + hardware_interface::ActuatorHandle vel_handle_a(act_state_interface.getHandle("Actuator"), &hw_commands_[1]); + act_vel_interface.registerHandle(vel_handle_a); + registerInterface(&act_vel_interface); + + // connect and register the actuator effort interface + hardware_interface::ActuatorHandle eff_handle_a(act_state_interface.getHandle("Actuator"), &hw_commands_[2]); + act_eff_interface.registerHandle(eff_handle_a); + registerInterface(&act_eff_interface); } bool TinymovrActuator::read(const ros::Duration& dt) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index af4b955..e1dc066 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -14,22 +14,22 @@ TinymovrHW::TinymovrJoint(): tm(1, send_cb, recv_cb) tmcan.init(); // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_a("Actuator", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); + hardware_interface::JointStateHandle state_handle_a("Joint", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); jnt_state_interface.registerHandle(state_handle_a); registerInterface(&jnt_state_interface); // connect and register the joint position interface - hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[0]); + hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("Joint"), &hw_commands_[0]); jnt_pos_interface.registerHandle(pos_handle_a); registerInterface(&jnt_pos_interface); // connect and register the joint velocity interface - hardware_interface::JointHandle vel_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[1]); + hardware_interface::JointHandle vel_handle_a(jnt_state_interface.getHandle("Joint"), &hw_commands_[1]); jnt_vel_interface.registerHandle(vel_handle_a); registerInterface(&jnt_vel_interface); // connect and register the joint effort interface - hardware_interface::JointHandle eff_handle_a(jnt_state_interface.getHandle("Actuator"), &hw_commands_[2]); + hardware_interface::JointHandle eff_handle_a(jnt_state_interface.getHandle("Joint"), &hw_commands_[2]); jnt_eff_interface.registerHandle(eff_handle_a); registerInterface(&jnt_eff_interface); } From a2bf8a9f87976d00c158a9e3cd2788a5f002367d Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:13:37 +0200 Subject: [PATCH 06/61] refactor joint interface --- include/tinymovr_can.hpp | 38 +++++++ include/tm_actuator_iface.hpp | 69 ------------- include/tm_joint_iface.hpp | 75 +++++--------- launch/tinymovr_actuator_iface_node.launch | 5 - launch/tinymovr_joint_iface_node.launch | 2 +- src/tm_actuator_iface.cpp | 51 ---------- src/tm_joint_iface.cpp | 113 +++++++++++++++------ src/tm_joint_iface_node.cpp | 28 ++--- 8 files changed, 162 insertions(+), 219 deletions(-) delete mode 100644 include/tm_actuator_iface.hpp delete mode 100644 launch/tinymovr_actuator_iface_node.launch delete mode 100644 src/tm_actuator_iface.cpp diff --git a/include/tinymovr_can.hpp b/include/tinymovr_can.hpp index 240d547..126ac46 100644 --- a/include/tinymovr_can.hpp +++ b/include/tinymovr_can.hpp @@ -7,6 +7,44 @@ namespace tinymovr_ros { +// --------------------------------------------------------------- +/* + * Function: send_cb + * -------------------- + * Is called to send a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be transmitted + * data_size: the size of transmitted data + * rtr: if the ftame is of request transmit type (RTR) + */ +void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +{ + if (!tmcan.write_frame(arbitration_id, data, data_size)) + { + throw "CAN write error"; + } +} + +/* + * Function: recv_cb + * -------------------- + * Is called to receive a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be received + * data_size: pointer to the variable that will hold the size of received data + */ +bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) +{ + (void)arbitration_id; + if (!tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_size)) + { + throw "CAN read error"; + } +} +// --------------------------------------------------------------- + class TinymovrCAN { public: diff --git a/include/tm_actuator_iface.hpp b/include/tm_actuator_iface.hpp deleted file mode 100644 index a656581..0000000 --- a/include/tm_actuator_iface.hpp +++ /dev/null @@ -1,69 +0,0 @@ - -#pragma once - -#include -#include -#include -#include -#include - -namespace tinymovr_ros -{ - -// --------------------------------------------------------------- -/* - * Function: send_cb - * -------------------- - * Is called to send a CAN frame - * - * arbitration_id: the frame arbitration id - * data: pointer to the data array to be transmitted - * data_size: the size of transmitted data - * rtr: if the ftame is of request transmit type (RTR) - */ -void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) -{ - return tmcan.write_frame(arbitration_id, data, data_size) -} - -/* - * Function: recv_cb - * -------------------- - * Is called to receive a CAN frame - * - * arbitration_id: the frame arbitration id - * data: pointer to the data array to be received - * data_size: pointer to the variable that will hold the size of received data - */ -bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) -{ - (void)arbitration_id; - return tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_size); -} -// --------------------------------------------------------------- - - -class TinymovrActuator : public hardware_interface::RobotHW -{ -public: - TinymovrActuator(); - bool read(const ros::Duration& period); - bool write(); - -private: - hardware_interface::ActuatorStateInterface act_state_interface; - hardware_interface::PositionActuatorInterface act_pos_interface; - hardware_interface::VelocityActuatorInterface act_vel_interface; - hardware_interface::EffortActuatorInterface act_eff_interface; - double cmd_pos; - double cmd_vel; - double cmd_eff; - double pos; - double vel; - double eff; - uint8_t node_id; - - Tinymovr tm; -}; - -} \ No newline at end of file diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index db09628..02d565d 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -10,60 +10,35 @@ namespace tinymovr_ros { -// --------------------------------------------------------------- -/* - * Function: send_cb - * -------------------- - * Is called to send a CAN frame - * - * arbitration_id: the frame arbitration id - * data: pointer to the data array to be transmitted - * data_size: the size of transmitted data - * rtr: if the ftame is of request transmit type (RTR) - */ -void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) -{ - return tmcan.write_frame(arbitration_id, data, data_size) -} - -/* - * Function: recv_cb - * -------------------- - * Is called to receive a CAN frame - * - * arbitration_id: the frame arbitration id - * data: pointer to the data array to be received - * data_size: pointer to the variable that will hold the size of received data - */ -bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) -{ - (void)arbitration_id; - return tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_size); -} -// --------------------------------------------------------------- - - class TinymovrJoint : public hardware_interface::RobotHW { public: TinymovrJoint(); - 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; - uint8_t node_id; - - Tinymovr tm; + ~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); + +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_names; + std::vector joint_ids; + + 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; }; } \ No newline at end of file diff --git a/launch/tinymovr_actuator_iface_node.launch b/launch/tinymovr_actuator_iface_node.launch deleted file mode 100644 index 0163c75..0000000 --- a/launch/tinymovr_actuator_iface_node.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/launch/tinymovr_joint_iface_node.launch b/launch/tinymovr_joint_iface_node.launch index 452139c..215414f 100644 --- a/launch/tinymovr_joint_iface_node.launch +++ b/launch/tinymovr_joint_iface_node.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/src/tm_actuator_iface.cpp b/src/tm_actuator_iface.cpp deleted file mode 100644 index b8dc0dd..0000000 --- a/src/tm_actuator_iface.cpp +++ /dev/null @@ -1,51 +0,0 @@ - -#include -#include -#include - -#include -#include -#include - -using namespace tinymovr_ros; - -TinymovrHW::TinymovrActuator(): tm(1, send_cb, recv_cb) -{ - tmcan.init(); - - // connect and register the actuator state interface - hardware_interface::ActuatorStateHandle state_handle_a("Actuator", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); - act_state_interface.registerHandle(state_handle_a); - registerInterface(&act_state_interface); - - // connect and register the actuator position interface - hardware_interface::ActuatorHandle pos_handle_a(act_state_interface.getHandle("Actuator"), &hw_commands_[0]); - act_pos_interface.registerHandle(pos_handle_a); - registerInterface(&act_pos_interface); - - // connect and register the actuator velocity interface - hardware_interface::ActuatorHandle vel_handle_a(act_state_interface.getHandle("Actuator"), &hw_commands_[1]); - act_vel_interface.registerHandle(vel_handle_a); - registerInterface(&act_vel_interface); - - // connect and register the actuator effort interface - hardware_interface::ActuatorHandle eff_handle_a(act_state_interface.getHandle("Actuator"), &hw_commands_[2]); - act_eff_interface.registerHandle(eff_handle_a); - registerInterface(&act_eff_interface); -} - -bool TinymovrActuator::read(const ros::Duration& dt) -{ - pos = tm.encoder.get_position_estimate(); - vel = tm.encoder.get_velocity_estimate(); - eff = tm.controller.current.get_Iq_estimate(); - return true; -} - -bool TinymovrActuator::write() -{ - tm.controller.position.set_setpoint(cmd_pos); - tm.controller.velocity.set_setpoint(cmd_vel); - tm.controller.current.set_Iq_setpoint(cmd_eff); - return true; -} \ No newline at end of file diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index e1dc066..b7c8b51 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -9,43 +9,98 @@ using namespace tinymovr_ros; -TinymovrHW::TinymovrJoint(): tm(1, send_cb, recv_cb) -{ +TinymovrJoint::TinymovrJoint() {} + +bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) +{ tmcan.init(); - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_a("Joint", &hw_positions_[0], &hw_velocities_[0], &hw_efforts_[0]); - jnt_state_interface.registerHandle(state_handle_a); - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("Joint"), &hw_commands_[0]); - jnt_pos_interface.registerHandle(pos_handle_a); - registerInterface(&jnt_pos_interface); - - // connect and register the joint velocity interface - hardware_interface::JointHandle vel_handle_a(jnt_state_interface.getHandle("Joint"), &hw_commands_[1]); - jnt_vel_interface.registerHandle(vel_handle_a); - registerInterface(&jnt_vel_interface); - - // connect and register the joint effort interface - hardware_interface::JointHandle eff_handle_a(jnt_state_interface.getHandle("Joint"), &hw_commands_[2]); - jnt_eff_interface.registerHandle(eff_handle_a); - registerInterface(&jnt_eff_interface); + //get joint names and num of joint + robot_hw_nh.getParam("joints/names", joint_names); + robot_hw_nh.getParam("joints/ids", joint_ids); + num_joints = joint_name.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); + + // initialize servos with correct mode + for (int i=0; i +#include #include #include int main(int argc, char **argv) { ros::init(argc, argv, "tinymovr_joint_iface_node"); - - tinymovr_ros::TinymovrJoint tm_joint; + + ros::AsyncSpinner spinner(1); + spinner.start(); ros::NodeHandle nh; - controller_manager::ControllerManager cm(&tm, nh); + tinymovr_ros::TinymovrJoint tm_joint; + bool init_success = hw.init(nh, nh); - ros::Rate rate(20.0); - ros::Time last; + controller_manager::ControllerManager cm(&hw, nh); - ros::AsyncSpinner spinner(2); - spinner.start(); + ros::Rate rate(100); // 100Hz update rate + ROS_INFO("tinymovr joint interface started"); while (ros::ok()) { - const ros::Time now = ros::Time::now(); - const ros::Duration period = now - last; - last = now; - tm_joint.read(period); - cm.update(now, period); - tm_joint.write(); + 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(); } + spinner.stop(); return 0; } From 0e621b0f6b0afdca00e7c0d78502f1fbe9873c89 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:16:36 +0200 Subject: [PATCH 07/61] update config --- config/hardware.yaml | 7 +++++++ config/tm_actuator_iface.yaml | 0 config/tm_joint_iface.yaml | 0 3 files changed, 7 insertions(+) create mode 100644 config/hardware.yaml delete mode 100644 config/tm_actuator_iface.yaml delete mode 100644 config/tm_joint_iface.yaml diff --git a/config/hardware.yaml b/config/hardware.yaml new file mode 100644 index 0000000..4091c6a --- /dev/null +++ b/config/hardware.yaml @@ -0,0 +1,7 @@ +joints: + names: + - joint1 + - joint2 + ids: + - 1 + - 2 \ No newline at end of file diff --git a/config/tm_actuator_iface.yaml b/config/tm_actuator_iface.yaml deleted file mode 100644 index e69de29..0000000 diff --git a/config/tm_joint_iface.yaml b/config/tm_joint_iface.yaml deleted file mode 100644 index e69de29..0000000 From 65c540947565ac58a4a86c2ff6f8e314ec6472b6 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:16:48 +0200 Subject: [PATCH 08/61] add build action --- .github/workflows/build.yaml | 52 ++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 .github/workflows/build.yaml diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml new file mode 100644 index 0000000..179a36a --- /dev/null +++ b/.github/workflows/build.yaml @@ -0,0 +1,52 @@ +name: Build on ROS Noetic Ubuntu 20.04 + +on: + push: + pull_request: + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-20.04 + env: + ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] + ROS_DISTRO: noetic + steps: + - uses: actions/checkout@v2 + - name: Install Non-ROS Dependencies + run: | + ls -l $GITHUB_WORKSPACE + - name: Install ROS + run: | + sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" + sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + sudo apt-get update -qq + sudo apt-get install dpkg + sudo apt-get install -y python3-catkin-pkg + sudo apt-get install -y python3-catkin-tools + sudo apt-get install -y python3-osrf-pycommon + sudo apt-get install -y python3-rosdep + sudo apt-get install -y python3-wstool + sudo apt-get install -y ros-cmake-modules + sudo apt-get install -y ros-$ROS_DISTRO-catkin + source /opt/ros/$ROS_DISTRO/setup.bash + - name: Install ROS packages with rosdep + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + sudo rosdep init + rosdep update + mkdir -p ~/catkin_ws/src + ln -s $GITHUB_WORKSPACE ~/catkin_ws/src/tinymovr_ros + cd ~/catkin_ws + rosdep install --from-paths src --ignore-src -r -s # do a dry-run first + rosdep install --from-paths src --ignore-src -r -y + - name: catkin build + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + cd ~/catkin_ws + catkin build --no-status + source devel/setup.bash + - name: lint + run: | + cd ~/catkin_ws + catkin build tinymovr_ros --no-deps --catkin-make-args roslint \ No newline at end of file From 717f8e442df2c388b85287bd046bb4314671b493 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:18:00 +0200 Subject: [PATCH 09/61] add rosin build --- .github/workflows/rosin_build.yaml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 .github/workflows/rosin_build.yaml diff --git a/.github/workflows/rosin_build.yaml b/.github/workflows/rosin_build.yaml new file mode 100644 index 0000000..36bb163 --- /dev/null +++ b/.github/workflows/rosin_build.yaml @@ -0,0 +1,16 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: noetic, ROS_REPO: testing} + - {ROS_DISTRO: noetic, 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 From 2637483e4c158934be70f08b309801f3e190e51e Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:21:44 +0200 Subject: [PATCH 10/61] remove unneeded build script --- .github/workflows/build.yaml | 52 ------------------------------------ 1 file changed, 52 deletions(-) delete mode 100644 .github/workflows/build.yaml diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml deleted file mode 100644 index 179a36a..0000000 --- a/.github/workflows/build.yaml +++ /dev/null @@ -1,52 +0,0 @@ -name: Build on ROS Noetic Ubuntu 20.04 - -on: - push: - pull_request: - workflow_dispatch: - -jobs: - build: - runs-on: ubuntu-20.04 - env: - ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...] - ROS_DISTRO: noetic - steps: - - uses: actions/checkout@v2 - - name: Install Non-ROS Dependencies - run: | - ls -l $GITHUB_WORKSPACE - - name: Install ROS - run: | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - sudo apt-get update -qq - sudo apt-get install dpkg - sudo apt-get install -y python3-catkin-pkg - sudo apt-get install -y python3-catkin-tools - sudo apt-get install -y python3-osrf-pycommon - sudo apt-get install -y python3-rosdep - sudo apt-get install -y python3-wstool - sudo apt-get install -y ros-cmake-modules - sudo apt-get install -y ros-$ROS_DISTRO-catkin - source /opt/ros/$ROS_DISTRO/setup.bash - - name: Install ROS packages with rosdep - run: | - source /opt/ros/$ROS_DISTRO/setup.bash - sudo rosdep init - rosdep update - mkdir -p ~/catkin_ws/src - ln -s $GITHUB_WORKSPACE ~/catkin_ws/src/tinymovr_ros - cd ~/catkin_ws - rosdep install --from-paths src --ignore-src -r -s # do a dry-run first - rosdep install --from-paths src --ignore-src -r -y - - name: catkin build - run: | - source /opt/ros/$ROS_DISTRO/setup.bash - cd ~/catkin_ws - catkin build --no-status - source devel/setup.bash - - name: lint - run: | - cd ~/catkin_ws - catkin build tinymovr_ros --no-deps --catkin-make-args roslint \ No newline at end of file From 03130fe121a078a63e48782daa06d7289825634b Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:27:10 +0200 Subject: [PATCH 11/61] update unit locations --- CMakeLists.txt | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a2d5254..6606ef5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,8 +131,7 @@ include_directories( # 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 src/socketcan_cpp.cpp src/tinymovr_can.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 +141,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(${PROJECT_NAME}_node src/tm_joint_iface_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -157,12 +155,7 @@ add_executable(${PROJECT_NAME}_diffbot_node src/diffbot_hw_iface_node.cpp) # 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 + ${PROJECT_NAME}_joint_iface ${catkin_LIBRARIES} ) From 69807a0b2ee4f9578501a1def04bfb8d5767c3d4 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:41:26 +0200 Subject: [PATCH 12/61] remove testing --- .github/workflows/rosin_build.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/rosin_build.yaml b/.github/workflows/rosin_build.yaml index 36bb163..99de1cc 100644 --- a/.github/workflows/rosin_build.yaml +++ b/.github/workflows/rosin_build.yaml @@ -7,7 +7,6 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: noetic, ROS_REPO: testing} - {ROS_DISTRO: noetic, ROS_REPO: main} runs-on: ubuntu-latest steps: From 63b8e18d34ed5a10cee9c3c5a159d4b957170606 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:41:32 +0200 Subject: [PATCH 13/61] fix import --- include/tm_joint_iface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 02d565d..45205b6 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include namespace tinymovr_ros { From f68c64282e874bdd7c33a220bf4d1ee995de47a9 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 15:51:25 +0200 Subject: [PATCH 14/61] refactor can --- include/tinymovr_can.hpp | 38 ------------------------------------ include/tm_joint_iface.hpp | 3 +++ src/tm_joint_iface.cpp | 40 +++++++++++++++++++++++++++++++++++++- 3 files changed, 42 insertions(+), 39 deletions(-) diff --git a/include/tinymovr_can.hpp b/include/tinymovr_can.hpp index 126ac46..240d547 100644 --- a/include/tinymovr_can.hpp +++ b/include/tinymovr_can.hpp @@ -7,44 +7,6 @@ namespace tinymovr_ros { -// --------------------------------------------------------------- -/* - * Function: send_cb - * -------------------- - * Is called to send a CAN frame - * - * arbitration_id: the frame arbitration id - * data: pointer to the data array to be transmitted - * data_size: the size of transmitted data - * rtr: if the ftame is of request transmit type (RTR) - */ -void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) -{ - if (!tmcan.write_frame(arbitration_id, data, data_size)) - { - throw "CAN write error"; - } -} - -/* - * Function: recv_cb - * -------------------- - * Is called to receive a CAN frame - * - * arbitration_id: the frame arbitration id - * data: pointer to the data array to be received - * data_size: pointer to the variable that will hold the size of received data - */ -bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) -{ - (void)arbitration_id; - if (!tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_size)) - { - throw "CAN read error"; - } -} -// --------------------------------------------------------------- - class TinymovrCAN { public: diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 45205b6..34085ed 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -20,6 +20,9 @@ class TinymovrJoint : public hardware_interface::RobotHW void write(const ros::Time& time, const ros::Duration& period); protected: + void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr); + bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size); + ros::NodeHandle nh_; hardware_interface::JointStateInterface joint_state_interface; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index b7c8b51..5e366b7 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -103,4 +103,42 @@ bool TinymovrJoint::write() ROS_ERROR_STREAM("Error while reading Tinymovr CAN:\n" << e.what()); return false; } -} \ No newline at end of file +} + +// --------------------------------------------------------------- +/* + * Function: send_cb + * -------------------- + * Is called to send a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be transmitted + * data_size: the size of transmitted data + * rtr: if the ftame is of request transmit type (RTR) + */ +void TinymovrJoint::send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +{ + if (!tmcan.write_frame(arbitration_id, data, data_size)) + { + throw "CAN write error"; + } +} + +/* + * Function: recv_cb + * -------------------- + * Is called to receive a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be received + * data_size: pointer to the variable that will hold the size of received data + */ +bool TinymovrJoint::recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) +{ + (void)arbitration_id; + if (!tmcan.read_frame(hw_node_ids_[i], CMD_GET_ENC_ESTIMATES, data, &data_size)) + { + throw "CAN read error"; + } +} +// --------------------------------------------------------------- \ No newline at end of file From fb1ac8b3e8187c487472b34224271313d2d13e82 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 16:25:31 +0200 Subject: [PATCH 15/61] refactor --- include/tm_joint_iface.hpp | 5 +++++ src/tm_joint_iface.cpp | 13 +++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 34085ed..8ab2839 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -6,6 +6,9 @@ #include #include #include +#include + +using namespace std; namespace tinymovr_ros { @@ -23,6 +26,8 @@ class TinymovrJoint : public hardware_interface::RobotHW void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr); bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size); + TinymovrCAN tmcan; + ros::NodeHandle nh_; hardware_interface::JointStateInterface joint_state_interface; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 5e366b7..aeefda7 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -7,7 +7,10 @@ #include #include -using namespace tinymovr_ros; +using namespace std; + +namespace tinymovr_ros +{ TinymovrJoint::TinymovrJoint() {} @@ -30,7 +33,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) // initialize servos with correct mode for (int i=0; i Date: Tue, 31 Jan 2023 16:32:21 +0200 Subject: [PATCH 16/61] add include dir --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6606ef5..214c711 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 From dd27556e14b7fc2caf77e152015be4204f7e9423 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 16:32:40 +0200 Subject: [PATCH 17/61] fix signature --- src/tinymovr_can.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp index be7e18d..55b185f 100644 --- a/src/tinymovr_can.cpp +++ b/src/tinymovr_can.cpp @@ -40,7 +40,7 @@ bool TinymovrCAN::read_frame(uint32_t node_id, uint32_t ep_id, uint8_t* data, ui bool TinymovrCAN::write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len) { - return write_frame(make_arbitration_id(node_id, ep_id)); + return write_frame(make_arbitration_id(node_id, ep_id), data, data_len); } bool TinymovrCAN::write_frame(uint32_t arbitration_id, const uint8_t *data, uint8_t data_len) From a01b46c5954bfbbecfe61dfa82cae3126812ed11 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 16:37:54 +0200 Subject: [PATCH 18/61] include --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 214c711..d1cab3b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,7 +122,7 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( - include + include include/tinymovr ${catkin_INCLUDE_DIRS} ) From 31a0e2fbbfd6a03579a13bbae5fef1fe530f913e Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Tue, 31 Jan 2023 16:47:22 +0200 Subject: [PATCH 19/61] fixes --- include/tinymovr/helpers.hpp | 2 + include/tm_joint_iface.hpp | 7 +-- src/tm_joint_iface.cpp | 92 +++++++++++++++++++----------------- 3 files changed, 52 insertions(+), 49 deletions(-) diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index 10a5200..e142579 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -49,6 +49,7 @@ class Node { bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us) { +#if defined ARDUINO // 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. @@ -56,6 +57,7 @@ class Node { { delayMicroseconds(delay_us); } +#endif const uint8_t arb_id = this->get_arbitration_id(cmd_id); return this->recv_cb(arb_id, data, data_size); } diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 8ab2839..da2aac6 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -6,7 +6,6 @@ #include #include #include -#include using namespace std; @@ -26,8 +25,6 @@ class TinymovrJoint : public hardware_interface::RobotHW void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr); bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size); - TinymovrCAN tmcan; - ros::NodeHandle nh_; hardware_interface::JointStateInterface joint_state_interface; @@ -36,8 +33,8 @@ class TinymovrJoint : public hardware_interface::RobotHW hardware_interface::EffortJointInterface joint_eff_interface; int num_joints; - std::vector joint_names; - std::vector joint_ids; + std::vector joint_name; + std::vector joint_id; std::vector joint_position_command; std::vector joint_velocity_command; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index aeefda7..d5de3be 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -5,6 +5,7 @@ #include #include +#include #include using namespace std; @@ -12,6 +13,47 @@ using namespace std; namespace tinymovr_ros { + +TinymovrCAN tmcan; + +// --------------------------------------------------------------- +/* + * Function: send_cb + * -------------------- + * Is called to send a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be transmitted + * data_size: the size of transmitted data + * rtr: if the ftame is of request transmit type (RTR) + */ +void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +{ + if (!tmcan.write_frame(arbitration_id, data, data_size)) + { + throw "CAN write error"; + } +} + +/* + * Function: recv_cb + * -------------------- + * Is called to receive a CAN frame + * + * arbitration_id: the frame arbitration id + * data: pointer to the data array to be received + * data_size: pointer to the variable that will hold the size of received data + */ +bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) +{ + (void)arbitration_id; + if (!tmcan.read_frame(arbitration_id, 0, data, &data_size)) + { + throw "CAN read error"; + } +} +// --------------------------------------------------------------- + TinymovrJoint::TinymovrJoint() {} bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) @@ -19,8 +61,8 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) tmcan.init(); //get joint names and num of joint - robot_hw_nh.getParam("joints/names", joint_names); - robot_hw_nh.getParam("joints/ids", joint_ids); + robot_hw_nh.getParam("joints/names", joint_name); + robot_hw_nh.getParam("joints/ids", joint_id); num_joints = joint_name.size(); joint_position_command.resize(num_joints, 0.0); @@ -33,7 +75,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) // initialize servos with correct mode for (int i=0; i Date: Tue, 31 Jan 2023 16:54:30 +0200 Subject: [PATCH 20/61] fixes --- include/tm_joint_iface.hpp | 3 --- src/tm_joint_iface.cpp | 8 +++++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index da2aac6..f6625d0 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -22,9 +22,6 @@ class TinymovrJoint : public hardware_interface::RobotHW void write(const ros::Time& time, const ros::Duration& period); protected: - void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr); - bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size); - ros::NodeHandle nh_; hardware_interface::JointStateInterface joint_state_interface; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index d5de3be..d904813 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -27,12 +27,13 @@ TinymovrCAN tmcan; * data_size: the size of transmitted data * rtr: if the ftame is of request transmit type (RTR) */ -void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +bool send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) { if (!tmcan.write_frame(arbitration_id, data, data_size)) { throw "CAN write error"; } + return true; } /* @@ -47,10 +48,11 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) { (void)arbitration_id; - if (!tmcan.read_frame(arbitration_id, 0, data, &data_size)) + if (!tmcan.read_frame(arbitration_id, 0, data, data_size)) { throw "CAN read error"; } + return true; } // --------------------------------------------------------------- @@ -75,7 +77,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) // initialize servos with correct mode for (int i=0; i Date: Tue, 31 Jan 2023 17:02:11 +0200 Subject: [PATCH 21/61] fixes --- src/tm_joint_iface.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index d904813..8a5ec8b 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -97,15 +97,15 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) joint_state_interface.registerHandle(state_handle); // connect and register the joint position interface - hardware_interface::JointHandle pos_handle(joint_state_interface.getHandle("Joint"), &cmd_pos_[i]); + hardware_interface::JointHandle pos_handle(joint_state_interface.getHandle("Joint"), &joint_position_command[i]); joint_pos_interface.registerHandle(pos_handle); // connect and register the joint velocity interface - hardware_interface::JointHandle vel_handle(joint_state_interface.getHandle("Joint"), &cmd_vel_[i]); + hardware_interface::JointHandle vel_handle(joint_state_interface.getHandle("Joint"), &joint_velocity_command[i]); joint_vel_interface.registerHandle(vel_handle); // connect and register the joint effort interface - hardware_interface::JointHandle eff_handle(joint_state_interface.getHandle("Joint"), &cmd_eff_[i]); + hardware_interface::JointHandle eff_handle(joint_state_interface.getHandle("Joint"), &joint_effort_command[i]); joint_eff_interface.registerHandle(eff_handle); } @@ -117,7 +117,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) return true; } -bool TinymovrJoint::read(const ros::Duration& dt) +bool TinymovrJoint::read(const ros::Time& time, const ros::Duration& period) { try { for (int i=0; i Date: Wed, 1 Feb 2023 16:46:40 +0200 Subject: [PATCH 22/61] fix function signatures --- include/tm_joint_iface.hpp | 4 ++-- src/tm_joint_iface.cpp | 11 +++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index f6625d0..ff0e93a 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -18,8 +18,8 @@ class TinymovrJoint : public hardware_interface::RobotHW 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); + bool read(const ros::Time& time, const ros::Duration& period); + bool write(const ros::Time& time, const ros::Duration& period); protected: ros::NodeHandle nh_; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 8a5ec8b..ecfa85f 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -25,15 +25,14 @@ TinymovrCAN tmcan; * arbitration_id: the frame arbitration id * data: pointer to the data array to be transmitted * data_size: the size of transmitted data - * rtr: if the ftame is of request transmit type (RTR) + * rtr: if the frame is of request transmit type (RTR) */ -bool send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) { if (!tmcan.write_frame(arbitration_id, data, data_size)) { throw "CAN write error"; } - return true; } /* @@ -77,12 +76,12 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) // initialize servos with correct mode for (int i=0; i Date: Wed, 1 Feb 2023 16:57:14 +0200 Subject: [PATCH 23/61] fix function signatures --- include/tm_joint_iface.hpp | 4 ++-- src/tm_joint_iface.cpp | 12 ++++-------- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index ff0e93a..cd57fa9 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -18,8 +18,8 @@ class TinymovrJoint : public hardware_interface::RobotHW TinymovrJoint(); ~TinymovrJoint(); bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh); - bool read(const ros::Time& time, const ros::Duration& period); - bool write(const ros::Time& time, const ros::Duration& period); + void read(const ros::Time& /*time*/, const ros::Duration& /*period*/); + void write(const ros::Time& /*time*/, const ros::Duration& /*period*/); protected: ros::NodeHandle nh_; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index ecfa85f..a98a527 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -116,39 +116,35 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) return true; } -bool TinymovrJoint::read(const ros::Time& time, const ros::Duration& period) +void TinymovrJoint::read(const ros::Time& time, const ros::Duration& period) { try { - for (int i=0; i Date: Wed, 1 Feb 2023 17:02:24 +0200 Subject: [PATCH 24/61] fix include --- include/tm_joint_iface.hpp | 1 + src/tm_joint_iface.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index cd57fa9..7f88c42 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -6,6 +6,7 @@ #include #include #include +#include using namespace std; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index a98a527..cdf5f15 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -5,7 +5,7 @@ #include #include -#include + #include using namespace std; From 89b0731938bcce99828493adf5a6b8492e828037 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Wed, 1 Feb 2023 17:02:29 +0200 Subject: [PATCH 25/61] fix ref --- src/tm_joint_iface_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tm_joint_iface_node.cpp b/src/tm_joint_iface_node.cpp index 6dd31bc..4d95e0d 100644 --- a/src/tm_joint_iface_node.cpp +++ b/src/tm_joint_iface_node.cpp @@ -11,7 +11,7 @@ int main(int argc, char **argv) spinner.start(); ros::NodeHandle nh; tinymovr_ros::TinymovrJoint tm_joint; - bool init_success = hw.init(nh, nh); + bool init_success = tm_joint.init(nh, nh); controller_manager::ControllerManager cm(&hw, nh); From ae4b5726f0b10c1a02ca677014fbac7f0d8a2f75 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 2 Feb 2023 10:24:58 +0200 Subject: [PATCH 26/61] fix reference --- src/tm_joint_iface_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tm_joint_iface_node.cpp b/src/tm_joint_iface_node.cpp index 4d95e0d..be0f030 100644 --- a/src/tm_joint_iface_node.cpp +++ b/src/tm_joint_iface_node.cpp @@ -13,7 +13,7 @@ int main(int argc, char **argv) tinymovr_ros::TinymovrJoint tm_joint; bool init_success = tm_joint.init(nh, nh); - controller_manager::ControllerManager cm(&hw, nh); + controller_manager::ControllerManager cm(&tm_joint, nh); ros::Rate rate(100); // 100Hz update rate From 3deae10b8d344a2029fab71d4b2a0c5f4746adc2 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 2 Feb 2023 11:02:01 +0200 Subject: [PATCH 27/61] add delete impl --- src/tm_joint_iface.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index cdf5f15..915c2ed 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -57,6 +57,8 @@ bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) TinymovrJoint::TinymovrJoint() {} +TinymovrJoint::~TinymovrJoint() {} + bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { tmcan.init(); From 3c5399c647fb2ba948fe5ae5c11e9759a869cc43 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 2 Feb 2023 11:08:51 +0200 Subject: [PATCH 28/61] add sources --- CMakeLists.txt | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d1cab3b..bc98fa0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,16 +122,33 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( - include include/tinymovr + 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}_joint_iface src/socketcan_cpp.cpp src/tinymovr_can.cpp src/tm_joint_iface.cpp) +add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp.cpp src/tinymovr_can.cpp src/tm_joint_iface.cpp) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries From ae96a148ea1ea9863aace137286e6bb38bbdf47f Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 2 Feb 2023 11:35:26 +0200 Subject: [PATCH 29/61] add int implementations --- include/tinymovr/helpers.hpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index e142579..e7dc40c 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -81,6 +81,12 @@ inline size_t write_le(uint8_t value, uint8_t* buffer) { 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; @@ -133,12 +139,25 @@ inline size_t read_le(bool* value, const uint8_t* buffer) { 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(uint8_t* value, const uint8_t* buffer) { *value = buffer[0]; return 1; } +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(uint16_t* value, const uint8_t* buffer) { *value = (static_cast(buffer[0]) << 0) | From d72fd8a378f31c5d58d76dc4f9302a0201036209 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 2 Feb 2023 11:44:44 +0200 Subject: [PATCH 30/61] update readme --- README.md | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 9f735aa..dbc4534 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,15 @@ +__Tinymovr ROS Hardware Interface__ -https://catkin-tools.readthedocs.io/en/latest/installing.html +ROS Hardware interface for working with Tinymovr motor controllers. -https://www.howtoinstall.me/ubuntu/18-04/liburdfdom-headers-dev/ +__Useful Resources__ -https://github.com/siposcsaba89/socketcan-cpp +These are not directly related to this repo, but were useful when trying to install ROS in a Raspbeery Pi -https://stackoverflow.com/questions/17808084/fatal-error-libudev-h-no-such-file-or-directory +Installing Catkin Tools: https://catkin-tools.readthedocs.io/en/latest/installing.html + +Installing liburdfdom: https://www.howtoinstall.me/ubuntu/18-04/liburdfdom-headers-dev/ + +Socketcan library: https://github.com/siposcsaba89/socketcan-cpp + +Issue with libudev: https://stackoverflow.com/questions/17808084/fatal-error-libudev-h-no-such-file-or-directory From 9cbbe837b83010282e132bd6152082014cc18a6f Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 2 Feb 2023 16:30:35 +0200 Subject: [PATCH 31/61] refactor config parsing --- config/hardware.yaml | 16 ++++++------ include/tm_joint_iface.hpp | 8 +++++- launch/tinymovr_joint_iface_node.launch | 2 +- src/tm_joint_iface.cpp | 33 +++++++++++++++++++++---- src/tm_joint_iface_node.cpp | 2 +- 5 files changed, 46 insertions(+), 15 deletions(-) diff --git a/config/hardware.yaml b/config/hardware.yaml index 4091c6a..a4972a3 100644 --- a/config/hardware.yaml +++ b/config/hardware.yaml @@ -1,7 +1,9 @@ -joints: - names: - - joint1 - - joint2 - ids: - - 1 - - 2 \ No newline at end of file +tinymovr_joint_iface: + joints: + arm_1: + # hardware ID of the actuator + id: 2 + offset: 3.14159265359 + # offset to be added, in radians, to the position of an actuator + #max-speed: 4.0 # in rad + command_interface: velocity \ No newline at end of file diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 7f88c42..94a68b2 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -1,10 +1,15 @@ #pragma once +// ROS +#include +#include +#include + #include #include #include -#include + #include #include @@ -42,6 +47,7 @@ class TinymovrJoint : public hardware_interface::RobotHW std::vector joint_effort_state; std::vector servos; + }; } \ No newline at end of file diff --git a/launch/tinymovr_joint_iface_node.launch b/launch/tinymovr_joint_iface_node.launch index 215414f..2c030dc 100644 --- a/launch/tinymovr_joint_iface_node.launch +++ b/launch/tinymovr_joint_iface_node.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 915c2ed..0e7d547 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -63,10 +63,33 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { tmcan.init(); - //get joint names and num of joint - robot_hw_nh.getParam("joints/names", joint_name); - robot_hw_nh.getParam("joints/ids", joint_id); - num_joints = joint_name.size(); + XmlRpc::XmlRpcValue servos_param; + + // build servo instances from configuration + if (got_all_params &= robot_hw_nh.getParam("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) { + + ROS_DEBUG_STREAM("servo: " << (std::string)(it->first)); + + id_t id; + if (it->second.hasMember("id")) + { + id = static_cast(servos_param[it->first]["id"]); + ROS_DEBUG_STREAM("\tid: " << (int)id); + servos.push_back(Tinymovr(id, &send_cb, &recv_cb)); + } + else + { + ROS_ERROR_STREAM("servo " << it->first << " has no associated servo ID."); + continue; + } + } + } + } + + num_joints = servos_param.size(); joint_position_command.resize(num_joints, 0.0); joint_velocity_command.resize(num_joints, 0.0); @@ -78,7 +101,6 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) // initialize servos with correct mode for (int i=0; i Date: Fri, 3 Feb 2023 11:43:59 +0200 Subject: [PATCH 32/61] fix --- src/tm_joint_iface.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 0e7d547..883c1b4 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -61,10 +61,11 @@ TinymovrJoint::~TinymovrJoint() {} bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { - tmcan.init(); - + XmlRpc::XmlRpcValue servos_param; + bool got_all_params = true; + // build servo instances from configuration if (got_all_params &= robot_hw_nh.getParam("joints", servos_param)) { ROS_ASSERT(servos_param.getType() == XmlRpc::XmlRpcValue::TypeStruct); @@ -87,6 +88,13 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) } } } + 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; + } } num_joints = servos_param.size(); @@ -98,6 +106,8 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) joint_velocity_state.resize(num_joints, 0.0); joint_effort_state.resize(num_joints, 0.0); + tmcan.init(); + // initialize servos with correct mode for (int i=0; i Date: Fri, 3 Feb 2023 12:29:04 +0200 Subject: [PATCH 33/61] include melodic --- .github/workflows/{rosin_build.yaml => industrial_ci.yaml} | 1 + 1 file changed, 1 insertion(+) rename .github/workflows/{rosin_build.yaml => industrial_ci.yaml} (85%) diff --git a/.github/workflows/rosin_build.yaml b/.github/workflows/industrial_ci.yaml similarity index 85% rename from .github/workflows/rosin_build.yaml rename to .github/workflows/industrial_ci.yaml index 99de1cc..5fdf2df 100644 --- a/.github/workflows/rosin_build.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -8,6 +8,7 @@ jobs: matrix: env: - {ROS_DISTRO: noetic, ROS_REPO: main} + - {ROS_DISTRO: melodic, ROS_REPO: main} runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 From 7c7999144e539de10d55e56cc8cae9a7b75ee8e8 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Wed, 8 Feb 2023 12:41:29 +0200 Subject: [PATCH 34/61] initiate correct operating modes --- include/tm_joint_iface.hpp | 1 + src/tm_joint_iface.cpp | 32 ++++++++++++++++++++++++++++++-- 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 94a68b2..93d1357 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -48,6 +48,7 @@ class TinymovrJoint : public hardware_interface::RobotHW std::vector servos; + uint8_t _str2mode(const std::string& mode_string); }; } \ No newline at end of file diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 883c1b4..f52774d 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -97,6 +97,15 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) } } + 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 + "/servos"; + ROS_FATAL_STREAM(error_message); + return false; + } + num_joints = servos_param.size(); joint_position_command.resize(num_joints, 0.0); @@ -113,7 +122,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { ROS_ASSERT((servos[i].encoder.get_calibrated() == true) && (servos[i].motor.get_calibrated() == true)); servos[i].controller.set_state(2); - servos[i].controller.set_mode(2); + servos[i].controller.set_mode(_str2mode()); ros::Duration(0.001).sleep(); ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == 2)); } @@ -151,6 +160,25 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) return true; } + /** Convert a string to an operating mode + @param mode_string name of the operating mode (current or effort, velocity or position) + @return mode index corresponding to the index of the mode + **/ + template + uint8_t TinymovrJoint::_str2mode(const std::string& mode_string) + { + if ("current" == mode_string || "effort" == mode_string) + return 0; + if ("velocity" == mode_string) + return 1; + else if ("position" == mode_string) + return 2; + else { + ROS_ERROR_STREAM("The command mode " << mode_string << " is not available"); + return 0; + } + } + void TinymovrJoint::read(const ros::Time& time, const ros::Duration& period) { try { @@ -179,7 +207,7 @@ void TinymovrJoint::write(const ros::Time& time, const ros::Duration& period) } catch(const std::exception& e) { - ROS_ERROR_STREAM("Error while reading Tinymovr CAN:\n" << e.what()); + ROS_ERROR_STREAM("Error while writing Tinymovr CAN:\n" << e.what()); } } From fc8204326ac2c9b00d5a611de26c197d2543740d Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Wed, 8 Feb 2023 16:32:11 +0200 Subject: [PATCH 35/61] update spec --- include/tinymovr/helpers.hpp | 21 +++++++++++++-------- include/tinymovr/tinymovr.hpp | 2 +- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index e7dc40c..e96c594 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -49,7 +49,6 @@ class Node { bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us) { -#if defined ARDUINO // 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. @@ -57,7 +56,6 @@ class Node { { delayMicroseconds(delay_us); } -#endif const uint8_t arb_id = this->get_arbitration_id(cmd_id); return this->recv_cb(arb_id, data, data_size); } @@ -94,6 +92,13 @@ inline size_t write_le(uint16_t value, uint8_t* buffer) { 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; @@ -140,26 +145,26 @@ inline size_t read_le(bool* value, const uint8_t* buffer) { } template<> -inline size_t read_le(int8_t* value, const uint8_t* buffer) { +inline size_t read_le(uint8_t* value, const uint8_t* buffer) { *value = buffer[0]; return 1; } template<> -inline size_t read_le(uint8_t* value, const uint8_t* buffer) { +inline size_t read_le(int8_t* value, const uint8_t* buffer) { *value = buffer[0]; return 1; } template<> -inline size_t read_le(int16_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8); +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(uint16_t* value, const uint8_t* buffer) { +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; diff --git a/include/tinymovr/tinymovr.hpp b/include/tinymovr/tinymovr.hpp index 1b09900..39bdf96 100644 --- a/include/tinymovr/tinymovr.hpp +++ b/include/tinymovr/tinymovr.hpp @@ -17,7 +17,7 @@ #include #include -static uint32_t avlos_proto_hash = 3273002564; +static uint32_t avlos_proto_hash = 2510016821; enum errors_flags { From 30a90bade751fc5d4f69ca1e31111151e831d666 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Wed, 8 Feb 2023 17:06:51 +0200 Subject: [PATCH 36/61] update spec --- src/tinymovr/controller.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/tinymovr/controller.cpp b/src/tinymovr/controller.cpp index 3254f19..54e7c6b 100644 --- a/src/tinymovr/controller.cpp +++ b/src/tinymovr/controller.cpp @@ -99,6 +99,13 @@ float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint) data_len += sizeof(vel_setpoint); this->send(40, this->_data, data_len, false); + float value = 0; + this->send(17, this->_data, 0, true); + if (this->recv(17, this->_data, &(this->_dlc), RECV_DELAY_US)) + { + read_le(&value, this->_data); + } + return value; } From f1a4a6e571581a17c4e081b619ba0e9403c6e501 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 9 Feb 2023 13:30:48 +0200 Subject: [PATCH 37/61] conditional --- include/tinymovr/helpers.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index e96c594..93a3b2b 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -49,6 +49,7 @@ class Node { bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us) { +#if defined ARDUINO // 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. @@ -56,6 +57,7 @@ class Node { { delayMicroseconds(delay_us); } +#endif const uint8_t arb_id = this->get_arbitration_id(cmd_id); return this->recv_cb(arb_id, data, data_size); } From d13901fae328855733ba2e7915d46d11ea780b24 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 9 Feb 2023 15:19:55 +0200 Subject: [PATCH 38/61] store servo mode strings --- include/tm_joint_iface.hpp | 1 + src/tm_joint_iface.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 93d1357..f02aa1d 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -47,6 +47,7 @@ class TinymovrJoint : public hardware_interface::RobotHW std::vector joint_effort_state; std::vector servos; + std::vector servo_modes; uint8_t _str2mode(const std::string& mode_string); }; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index f52774d..0394ede 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -80,6 +80,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) id = static_cast(servos_param[it->first]["id"]); ROS_DEBUG_STREAM("\tid: " << (int)id); servos.push_back(Tinymovr(id, &send_cb, &recv_cb)); + servo_modes.push_back(servos_param[it->first]["command_interface"]) } else { @@ -122,7 +123,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { ROS_ASSERT((servos[i].encoder.get_calibrated() == true) && (servos[i].motor.get_calibrated() == true)); servos[i].controller.set_state(2); - servos[i].controller.set_mode(_str2mode()); + servos[i].controller.set_mode(_str2mode(servo_modes[i])); ros::Duration(0.001).sleep(); ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == 2)); } From ac82b2f2af6b1c345ad894e30fbad791ff06035f Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 9 Feb 2023 15:24:43 +0200 Subject: [PATCH 39/61] fix --- src/tm_joint_iface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 0394ede..8d17b38 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -80,7 +80,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) id = static_cast(servos_param[it->first]["id"]); ROS_DEBUG_STREAM("\tid: " << (int)id); servos.push_back(Tinymovr(id, &send_cb, &recv_cb)); - servo_modes.push_back(servos_param[it->first]["command_interface"]) + servo_modes.push_back(servos_param[it->first]["command_interface"]); } else { @@ -166,7 +166,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) @return mode index corresponding to the index of the mode **/ template - uint8_t TinymovrJoint::_str2mode(const std::string& mode_string) + uint8_t TinymovrJoint::_str2mode(std::string& mode_string) { if ("current" == mode_string || "effort" == mode_string) return 0; From 12a56c688f79105a7377f56f745e6353c25f745c Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 9 Feb 2023 15:31:38 +0200 Subject: [PATCH 40/61] fix declaration --- include/tm_joint_iface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index f02aa1d..ec00d5b 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -49,7 +49,7 @@ class TinymovrJoint : public hardware_interface::RobotHW std::vector servos; std::vector servo_modes; - uint8_t _str2mode(const std::string& mode_string); + uint8_t _str2mode(std::string& mode_string); }; } \ No newline at end of file From 3c61215e6d31a8073498ed5c9e399c8db1ae450c Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 9 Feb 2023 15:43:45 +0200 Subject: [PATCH 41/61] remove template --- src/tm_joint_iface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 8d17b38..6012cc0 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -165,7 +165,6 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) @param mode_string name of the operating mode (current or effort, velocity or position) @return mode index corresponding to the index of the mode **/ - template uint8_t TinymovrJoint::_str2mode(std::string& mode_string) { if ("current" == mode_string || "effort" == mode_string) From 7cb6b9eb2417d065f6452697e4d2be4db01e7a08 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 30 Mar 2023 14:33:04 +0300 Subject: [PATCH 42/61] update protocol --- include/tinymovr/can.hpp | 4 ++-- include/tinymovr/comms.hpp | 6 +++--- include/tinymovr/controller.hpp | 12 ++++++------ include/tinymovr/current.hpp | 4 ++-- include/tinymovr/encoder.hpp | 4 ++-- include/tinymovr/helpers.hpp | 26 +++++++++++++++++++------- include/tinymovr/motor.hpp | 4 ++-- include/tinymovr/position.hpp | 4 ++-- include/tinymovr/scheduler.hpp | 4 ++-- include/tinymovr/tinymovr.hpp | 20 ++++++++++---------- include/tinymovr/traj_planner.hpp | 4 ++-- include/tinymovr/velocity.hpp | 4 ++-- include/tinymovr/voltage.hpp | 4 ++-- include/tinymovr/watchdog.hpp | 4 ++-- 14 files changed, 58 insertions(+), 46 deletions(-) diff --git a/include/tinymovr/can.hpp b/include/tinymovr/can.hpp index 4dd3a32..d654aca 100644 --- a/include/tinymovr/can.hpp +++ b/include/tinymovr/can.hpp @@ -14,8 +14,8 @@ class Can_ : Node { public: - Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; uint32_t get_rate(void); void set_rate(uint32_t value); uint32_t get_id(void); diff --git a/include/tinymovr/comms.hpp b/include/tinymovr/comms.hpp index 4db122a..50e90b3 100644 --- a/include/tinymovr/comms.hpp +++ b/include/tinymovr/comms.hpp @@ -15,9 +15,9 @@ class Comms_ : Node { public: - Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) - , can(_can_node_id, _send_cb, _recv_cb) {}; + Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , can(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; Can_ can; }; diff --git a/include/tinymovr/controller.hpp b/include/tinymovr/controller.hpp index bb410b8..a6aeb9a 100644 --- a/include/tinymovr/controller.hpp +++ b/include/tinymovr/controller.hpp @@ -18,12 +18,12 @@ class Controller_ : Node { public: - Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) - , position(_can_node_id, _send_cb, _recv_cb) - , velocity(_can_node_id, _send_cb, _recv_cb) - , current(_can_node_id, _send_cb, _recv_cb) - , voltage(_can_node_id, _send_cb, _recv_cb) {}; + Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , position(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , velocity(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , current(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , voltage(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; uint8_t get_state(void); void set_state(uint8_t value); uint8_t get_mode(void); diff --git a/include/tinymovr/current.hpp b/include/tinymovr/current.hpp index 2fe667f..d065c2e 100644 --- a/include/tinymovr/current.hpp +++ b/include/tinymovr/current.hpp @@ -14,8 +14,8 @@ class Current_ : Node { public: - Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; float get_Iq_setpoint(void); void set_Iq_setpoint(float value); float get_Id_setpoint(void); diff --git a/include/tinymovr/encoder.hpp b/include/tinymovr/encoder.hpp index edc79fe..c9a1e03 100644 --- a/include/tinymovr/encoder.hpp +++ b/include/tinymovr/encoder.hpp @@ -14,8 +14,8 @@ class Encoder_ : Node { public: - Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; float get_position_estimate(void); float get_velocity_estimate(void); uint8_t get_type(void); diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index 93a3b2b..9612f72 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -24,18 +24,20 @@ #define RECV_DELAY_US (160.0f) 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 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): - can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb) {} + Node(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb), delay_us_cb(_delay_us_cb) {} protected: uint8_t can_node_id; send_callback send_cb; recv_callback recv_cb; + delay_us_callback delay_us_cb; uint8_t _data[8]; uint8_t _dlc; uint8_t get_arbitration_id(uint8_t cmd_id) { @@ -49,17 +51,27 @@ class Node { bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us) { -#if defined ARDUINO + 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) { - delayMicroseconds(delay_us); + this->delay_us_cb(delay_us); } -#endif const uint8_t arb_id = this->get_arbitration_id(cmd_id); - return this->recv_cb(arb_id, data, data_size); + 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; } }; diff --git a/include/tinymovr/motor.hpp b/include/tinymovr/motor.hpp index fff17c0..f05c1cf 100644 --- a/include/tinymovr/motor.hpp +++ b/include/tinymovr/motor.hpp @@ -14,8 +14,8 @@ class Motor_ : Node { public: - Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; float get_R(void); void set_R(float value); float get_L(void); diff --git a/include/tinymovr/position.hpp b/include/tinymovr/position.hpp index 451afe9..946f420 100644 --- a/include/tinymovr/position.hpp +++ b/include/tinymovr/position.hpp @@ -14,8 +14,8 @@ class Position_ : Node { public: - Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; float get_setpoint(void); void set_setpoint(float value); float get_p_gain(void); diff --git a/include/tinymovr/scheduler.hpp b/include/tinymovr/scheduler.hpp index a1d9bd5..18e5dce 100644 --- a/include/tinymovr/scheduler.hpp +++ b/include/tinymovr/scheduler.hpp @@ -14,8 +14,8 @@ class Scheduler_ : Node { public: - Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; uint32_t get_total(void); uint32_t get_busy(void); uint8_t get_errors(void); diff --git a/include/tinymovr/tinymovr.hpp b/include/tinymovr/tinymovr.hpp index 39bdf96..031b8b0 100644 --- a/include/tinymovr/tinymovr.hpp +++ b/include/tinymovr/tinymovr.hpp @@ -17,7 +17,7 @@ #include #include -static uint32_t avlos_proto_hash = 2510016821; +static uint32_t avlos_proto_hash = 3273002564; enum errors_flags { @@ -72,15 +72,15 @@ class Tinymovr : Node { public: - Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) - , scheduler(_can_node_id, _send_cb, _recv_cb) - , controller(_can_node_id, _send_cb, _recv_cb) - , comms(_can_node_id, _send_cb, _recv_cb) - , motor(_can_node_id, _send_cb, _recv_cb) - , encoder(_can_node_id, _send_cb, _recv_cb) - , traj_planner(_can_node_id, _send_cb, _recv_cb) - , watchdog(_can_node_id, _send_cb, _recv_cb) {}; + Tinymovr(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , motor(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , encoder(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , traj_planner(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) + , watchdog(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; uint32_t get_protocol_hash(void); uint32_t get_uid(void); float get_Vbus(void); diff --git a/include/tinymovr/traj_planner.hpp b/include/tinymovr/traj_planner.hpp index 0901cbf..cd751d9 100644 --- a/include/tinymovr/traj_planner.hpp +++ b/include/tinymovr/traj_planner.hpp @@ -14,8 +14,8 @@ class Traj_planner_ : Node { public: - Traj_planner_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Traj_planner_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; float get_max_accel(void); void set_max_accel(float value); float get_max_decel(void); diff --git a/include/tinymovr/velocity.hpp b/include/tinymovr/velocity.hpp index a8525ff..50bf210 100644 --- a/include/tinymovr/velocity.hpp +++ b/include/tinymovr/velocity.hpp @@ -14,8 +14,8 @@ class Velocity_ : Node { public: - Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; float get_setpoint(void); void set_setpoint(float value); float get_limit(void); diff --git a/include/tinymovr/voltage.hpp b/include/tinymovr/voltage.hpp index ed19750..1a4f10d 100644 --- a/include/tinymovr/voltage.hpp +++ b/include/tinymovr/voltage.hpp @@ -14,8 +14,8 @@ class Voltage_ : Node { public: - Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; float get_Vq_setpoint(void); }; diff --git a/include/tinymovr/watchdog.hpp b/include/tinymovr/watchdog.hpp index 19d8180..a2f9693 100644 --- a/include/tinymovr/watchdog.hpp +++ b/include/tinymovr/watchdog.hpp @@ -14,8 +14,8 @@ class Watchdog_ : Node { public: - Watchdog_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb): - Node(_can_node_id, _send_cb, _recv_cb) {}; + Watchdog_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): + Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; bool get_enabled(void); void set_enabled(bool value); bool get_triggered(void); From 6e4f397bde3ec90985078641d42958b513c95749 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 30 Mar 2023 14:54:49 +0300 Subject: [PATCH 43/61] update signatures --- src/tm_joint_iface.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 6012cc0..ba65009 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -44,7 +44,7 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr * data: pointer to the data array to be received * data_size: pointer to the variable that will hold the size of received data */ -bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) +bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_size) { (void)arbitration_id; if (!tmcan.read_frame(arbitration_id, 0, data, data_size)) @@ -53,6 +53,18 @@ bool recv_cb(uint32_t arbitration_id, uint8_t *data, uint8_t *data_size) } return true; } + +/* + * Function: delay_us_cb + * -------------------- + * Is called to perform a delay + * + * us: the microseconds to wait for + */ +void delay_us_cb(uint32_t us) +{ + ros::Duration(us * 1e-6).sleep(); +} // --------------------------------------------------------------- TinymovrJoint::TinymovrJoint() {} @@ -79,7 +91,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { id = static_cast(servos_param[it->first]["id"]); ROS_DEBUG_STREAM("\tid: " << (int)id); - servos.push_back(Tinymovr(id, &send_cb, &recv_cb)); + servos.push_back(Tinymovr(id, &send_cb, &recv_cb, &delay_us_cb)); servo_modes.push_back(servos_param[it->first]["command_interface"]); } else From 504dd361fe91aa4ff2f6405b5c773ffa83b86370 Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 30 Mar 2023 15:01:09 +0300 Subject: [PATCH 44/61] include --- include/tinymovr/helpers.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index 9612f72..09d9dc1 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -14,6 +14,7 @@ #else #include #include +#include #endif #if defined ARDUINO From 69fdec04bdad6e84423d1c4494b0462c402a926a Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 30 Mar 2023 15:16:44 +0300 Subject: [PATCH 45/61] fix callbacks --- include/tinymovr_can.hpp | 2 +- src/tinymovr_can.cpp | 10 +++------- src/tm_joint_iface.cpp | 5 ++--- 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/include/tinymovr_can.hpp b/include/tinymovr_can.hpp index 240d547..12a92c3 100644 --- a/include/tinymovr_can.hpp +++ b/include/tinymovr_can.hpp @@ -11,7 +11,7 @@ class TinymovrCAN { public: void init(); - bool read_frame(uint32_t node_id, uint32_t ep_id, uint8_t* data, uint8_t* data_len); + bool read_frame(uint32_t *arbitration_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); bool write_frame(uint32_t arbitration_id, const uint8_t *data, uint8_t data_len); uint32_t make_arbitration_id(uint32_t node_id, uint32_t command_id); diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp index 55b185f..155d0a1 100644 --- a/src/tinymovr_can.cpp +++ b/src/tinymovr_can.cpp @@ -18,13 +18,8 @@ void TinymovrCAN::init() } } -bool TinymovrCAN::read_frame(uint32_t node_id, uint32_t ep_id, uint8_t* data, uint8_t* data_len) +bool TinymovrCAN::read_frame(uint32_t *arbitration_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)) @@ -32,8 +27,9 @@ bool TinymovrCAN::read_frame(uint32_t node_id, uint32_t ep_id, uint8_t* data, ui return false; } - memcpy(data, fr.data, 8); // TODO: make safer + memcpy(data, fr.data, 8); *data_len = fr.len; + *arbitration_id = fr.id; return true; } diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index ba65009..9f69efe 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -40,14 +40,13 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr * -------------------- * Is called to receive a CAN frame * - * arbitration_id: the frame arbitration id + * arbitration_id: the frame arbitration id pointer * data: pointer to the data array to be received * data_size: pointer to the variable that will hold the size of received data */ bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_size) { - (void)arbitration_id; - if (!tmcan.read_frame(arbitration_id, 0, data, data_size)) + if (!tmcan.read_frame(arbitration_id, data, data_size)) { throw "CAN read error"; } From 03dee75d0c9c5a29fcaaa450fa012bec4fa0be6f Mon Sep 17 00:00:00 2001 From: Ioannis Chatzikonstantinou Date: Thu, 30 Mar 2023 15:21:28 +0300 Subject: [PATCH 46/61] typo --- src/tinymovr_can.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp index 155d0a1..fa2f804 100644 --- a/src/tinymovr_can.cpp +++ b/src/tinymovr_can.cpp @@ -18,7 +18,7 @@ void TinymovrCAN::init() } } -bool TinymovrCAN::read_frame(uint32_t *arbitration_id uint8_t *data, uint8_t *data_len) +bool TinymovrCAN::read_frame(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_len) { scpp::CanFrame fr; From 0c61876d6b631a418dea7b30d8adaa486a1631c4 Mon Sep 17 00:00:00 2001 From: aman-pandey23 Date: Sun, 23 Jul 2023 05:30:11 +0530 Subject: [PATCH 47/61] resolved build issue in cmake --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bc98fa0..67c2708 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,7 +158,7 @@ add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp.cp ## 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_joint_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 @@ -171,7 +171,7 @@ add_executable(${PROJECT_NAME}_node src/tm_joint_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 +target_link_libraries(tinymovr_joint_iface_node ${PROJECT_NAME}_joint_iface ${catkin_LIBRARIES} ) From 043c19e9f951fa49fd99cd3d67d85a6a0e2bdb63 Mon Sep 17 00:00:00 2001 From: aman-pandey23 Date: Tue, 5 Sep 2023 14:17:02 +0530 Subject: [PATCH 48/61] can error debug --- config/velocity_controller.yaml | 4 +++ include/socketcan_cpp/socketcan_cpp.hpp | 2 +- launch/tinymovr_joint_iface_node.launch | 3 +++ src/socketcan_cpp.cpp | 20 ++++++++++++-- src/tinymovr_can.cpp | 36 +++++++++++++++++++------ src/tm_joint_iface.cpp | 19 +++++++++---- 6 files changed, 68 insertions(+), 16 deletions(-) create mode 100644 config/velocity_controller.yaml diff --git a/config/velocity_controller.yaml b/config/velocity_controller.yaml new file mode 100644 index 0000000..8fb8f98 --- /dev/null +++ b/config/velocity_controller.yaml @@ -0,0 +1,4 @@ +arm_controller: + type: "velocity_controllers/JointVelocityController" + joint: arm_1 + pid: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file 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/launch/tinymovr_joint_iface_node.launch b/launch/tinymovr_joint_iface_node.launch index 2c030dc..317a05e 100644 --- a/launch/tinymovr_joint_iface_node.launch +++ b/launch/tinymovr_joint_iface_node.launch @@ -1,5 +1,8 @@ + + \ No newline at end of file diff --git a/src/socketcan_cpp.cpp b/src/socketcan_cpp.cpp index 1084a68..d8b8596 100644 --- a/src/socketcan_cpp.cpp +++ b/src/socketcan_cpp.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #ifdef HAVE_SOCKETCAN_HEADERS #include @@ -54,6 +55,7 @@ namespace scpp } SocketCanStatus SocketCan::open(const std::string & can_interface, int32_t read_timeout_ms, SocketMode mode) { + ROS_INFO("Opening SocketCAN on interface: %s", can_interface.c_str()); m_interface = can_interface; m_socket_mode = mode; m_read_timeout_ms = read_timeout_ms; @@ -128,6 +130,14 @@ namespace scpp perror("bind"); return STATUS_BIND_ERROR; } + ROS_INFO("SocketCAN opened successfully on interface: %s", can_interface.c_str()); + 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 @@ -135,6 +145,7 @@ namespace scpp } SocketCanStatus SocketCan::write(const CanFrame & msg) { + ROS_INFO("Writing frame with ID: %d", msg.id); #ifdef HAVE_SOCKETCAN_HEADERS struct canfd_frame frame; memset(&frame, 0, sizeof(frame)); /* init CAN FD frame, e.g. LEN = 0 */ @@ -154,6 +165,7 @@ namespace scpp perror("write"); return STATUS_WRITE_ERROR; } + ROS_INFO("Frame with ID: %d written successfully.", msg.id); #else printf("Your operating system does not support socket can! \n"); #endif @@ -161,6 +173,7 @@ namespace scpp } SocketCanStatus SocketCan::read(CanFrame & msg) { + ROS_INFO("Reading frame..."); #ifdef HAVE_SOCKETCAN_HEADERS struct canfd_frame frame; @@ -168,10 +181,11 @@ namespace scpp auto num_bytes = ::read(m_socket, &frame, CANFD_MTU); if (num_bytes != CAN_MTU && num_bytes != CANFD_MTU) { + ROS_ERROR("Error reading frame."); //perror("Can read error"); return STATUS_READ_ERROR; } - + ROS_INFO("Read frame with ID: %d successfully.", msg.id); msg.id = frame.can_id; msg.len = frame.len; msg.flags = frame.flags; @@ -183,9 +197,11 @@ namespace scpp } SocketCanStatus SocketCan::close() { + ROS_INFO("Closing SocketCAN on interface: %s", m_interface.c_str()); #ifdef HAVE_SOCKETCAN_HEADERS ::close(m_socket); #endif + ROS_INFO("SocketCAN closed successfully."); return STATUS_OK; } const std::string & SocketCan::interfaceName() const @@ -196,4 +212,4 @@ namespace scpp { close(); } -} +} \ No newline at end of file diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp index fa2f804..df25023 100644 --- a/src/tinymovr_can.cpp +++ b/src/tinymovr_can.cpp @@ -1,5 +1,7 @@ #include +#include + namespace tinymovr_ros { @@ -18,29 +20,47 @@ void TinymovrCAN::init() } } -bool TinymovrCAN::read_frame(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_len) +bool TinymovrCAN::read_frame(uint32_t* can_id, uint8_t* data, uint8_t* len) { + ROS_INFO("in read frame"); scpp::CanFrame fr; - - if (scpp::STATUS_OK != socket_can.read(fr)) + scpp::SocketCanStatus read_status = socket_can.read(fr); + if (read_status == scpp::STATUS_OK) + { + *can_id = fr.id; + *len = fr.len; + std::copy(fr.data, fr.data + fr.len, data); + ROS_DEBUG("Successfully read a CAN frame"); + return true; + } + else { + ROS_WARN("Failed to read a CAN frame. SocketCanStatus: %d", static_cast(read_status)); + switch(read_status) + { + case scpp::STATUS_READ_ERROR: + ROS_ERROR("SocketCan read error!"); + break; + // Removed STATUS_TIMEOUT case + default: + ROS_ERROR("Unknown SocketCan error!"); + break; + } return false; } +} - memcpy(data, fr.data, 8); - *data_len = fr.len; - *arbitration_id = fr.id; - return true; -} bool TinymovrCAN::write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len) { + ROS_INFO("in write frame1"); return write_frame(make_arbitration_id(node_id, ep_id), data, data_len); } bool TinymovrCAN::write_frame(uint32_t arbitration_id, const uint8_t *data, uint8_t data_len) { + ROS_INFO("in write frame2"); scpp::CanFrame cf_to_write; cf_to_write.id = arbitration_id; diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 9f69efe..39a8ac0 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -5,6 +5,7 @@ #include #include +#include #include @@ -29,10 +30,14 @@ TinymovrCAN tmcan; */ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) { + ROS_DEBUG_STREAM("Attempting to write CAN frame with arbitration_id: " << arbitration_id); + if (!tmcan.write_frame(arbitration_id, data, data_size)) { - throw "CAN write error"; + throw std::runtime_error("CAN write error"); } + + ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << arbitration_id << " written successfully."); } /* @@ -46,10 +51,14 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr */ bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_size) { + ROS_DEBUG_STREAM("Attempting to read CAN frame..."); + if (!tmcan.read_frame(arbitration_id, data, data_size)) { - throw "CAN read error"; + throw std::runtime_error("CAN read error"); } + + ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << *arbitration_id << " read successfully."); return true; } @@ -78,7 +87,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) bool got_all_params = true; // build servo instances from configuration - if (got_all_params &= robot_hw_nh.getParam("joints", servos_param)) { + 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) { @@ -112,8 +121,8 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) 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 + "/servos"; + "were not set:\n" + + sub_namespace + "/tinymovr_joint_iface/joints"; ROS_FATAL_STREAM(error_message); return false; } From 67de9951dbd83c6f956954b1fcb88885027fe8d5 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Thu, 14 Sep 2023 23:56:56 +0300 Subject: [PATCH 49/61] update protocol --- include/tinymovr/can.hpp | 4 +- include/tinymovr/comms.hpp | 6 +-- include/tinymovr/controller.hpp | 12 ++--- include/tinymovr/current.hpp | 4 +- include/tinymovr/encoder.hpp | 4 +- include/tinymovr/helpers.hpp | 29 +++++++----- include/tinymovr/homing.hpp | 31 ++++++++++++ include/tinymovr/motor.hpp | 4 +- include/tinymovr/position.hpp | 4 +- include/tinymovr/scheduler.hpp | 6 +-- include/tinymovr/stall_detect.hpp | 26 ++++++++++ include/tinymovr/tinymovr.hpp | 51 +++++++++++++++----- include/tinymovr/traj_planner.hpp | 10 +++- include/tinymovr/velocity.hpp | 4 +- include/tinymovr/voltage.hpp | 4 +- include/tinymovr/watchdog.hpp | 4 +- src/tinymovr/can.cpp | 12 ++--- src/tinymovr/controller.cpp | 34 ++++++------- src/tinymovr/current.cpp | 42 ++++++++-------- src/tinymovr/encoder.cpp | 28 +++++------ src/tinymovr/homing.cpp | 79 +++++++++++++++++++++++++++++++ src/tinymovr/motor.cpp | 50 +++++++++---------- src/tinymovr/position.cpp | 12 ++--- src/tinymovr/scheduler.cpp | 26 +--------- src/tinymovr/stall_detect.cpp | 63 ++++++++++++++++++++++++ src/tinymovr/tinymovr.cpp | 58 ++++++++++++++++------- src/tinymovr/traj_planner.cpp | 77 +++++++++++++++++++++++++----- src/tinymovr/velocity.cpp | 36 +++++++------- src/tinymovr/voltage.cpp | 4 +- src/tinymovr/watchdog.cpp | 16 +++---- 30 files changed, 514 insertions(+), 226 deletions(-) create mode 100644 include/tinymovr/homing.hpp create mode 100644 include/tinymovr/stall_detect.hpp create mode 100644 src/tinymovr/homing.cpp create mode 100644 src/tinymovr/stall_detect.cpp diff --git a/include/tinymovr/can.hpp b/include/tinymovr/can.hpp index d654aca..6f44d56 100644 --- a/include/tinymovr/can.hpp +++ b/include/tinymovr/can.hpp @@ -14,8 +14,8 @@ class Can_ : Node { public: - Can_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/include/tinymovr/comms.hpp b/include/tinymovr/comms.hpp index 50e90b3..813d232 100644 --- a/include/tinymovr/comms.hpp +++ b/include/tinymovr/comms.hpp @@ -15,9 +15,9 @@ class Comms_ : Node { public: - Comms_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , can(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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 index a6aeb9a..e8b4e09 100644 --- a/include/tinymovr/controller.hpp +++ b/include/tinymovr/controller.hpp @@ -18,12 +18,12 @@ class Controller_ : Node { public: - Controller_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , position(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , velocity(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , current(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , voltage(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/include/tinymovr/current.hpp b/include/tinymovr/current.hpp index d065c2e..a6e2ba6 100644 --- a/include/tinymovr/current.hpp +++ b/include/tinymovr/current.hpp @@ -14,8 +14,8 @@ class Current_ : Node { public: - Current_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/include/tinymovr/encoder.hpp b/include/tinymovr/encoder.hpp index c9a1e03..d05d2e4 100644 --- a/include/tinymovr/encoder.hpp +++ b/include/tinymovr/encoder.hpp @@ -14,8 +14,8 @@ class Encoder_ : Node { public: - Encoder_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index 09d9dc1..e69a122 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -14,15 +14,18 @@ #else #include #include -#include #endif #if defined ARDUINO #include "Arduino.h" #endif -#define EP_BITS (6) -#define RECV_DELAY_US (160.0f) +#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); @@ -31,26 +34,28 @@ 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): - can_node_id(_can_node_id), send_cb(_send_cb), recv_cb(_recv_cb), delay_us_cb(_delay_us_cb) {} + 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; - uint8_t get_arbitration_id(uint8_t cmd_id) { - return this->can_node_id << EP_BITS | cmd_id; + 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(uint8_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr) + void send(uint32_t cmd_id, uint8_t *data, uint8_t data_size, bool rtr) { - const uint8_t arb_id = this->get_arbitration_id(cmd_id); + const uint32_t arb_id = this->get_arbitration_id(cmd_id); this->send_cb(arb_id, data, data_size, rtr); } - bool recv(uint8_t cmd_id, uint8_t *data, uint8_t *data_size, uint16_t delay_us) + 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]; @@ -62,7 +67,7 @@ class Node { { this->delay_us_cb(delay_us); } - const uint8_t arb_id = this->get_arbitration_id(cmd_id); + 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) @@ -218,8 +223,6 @@ inline size_t read_le(uint64_t* value, const uint8_t* buffer) { template<> inline size_t read_le(float* value, const 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"); return read_le(reinterpret_cast(value), buffer); } 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 index f05c1cf..ce298bf 100644 --- a/include/tinymovr/motor.hpp +++ b/include/tinymovr/motor.hpp @@ -14,8 +14,8 @@ class Motor_ : Node { public: - Motor_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/include/tinymovr/position.hpp b/include/tinymovr/position.hpp index 946f420..3ea3937 100644 --- a/include/tinymovr/position.hpp +++ b/include/tinymovr/position.hpp @@ -14,8 +14,8 @@ class Position_ : Node { public: - Position_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/include/tinymovr/scheduler.hpp b/include/tinymovr/scheduler.hpp index 18e5dce..2fe9bbc 100644 --- a/include/tinymovr/scheduler.hpp +++ b/include/tinymovr/scheduler.hpp @@ -14,10 +14,8 @@ class Scheduler_ : Node { public: - Scheduler_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; - uint32_t get_total(void); - uint32_t get_busy(void); + 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 index 031b8b0..400e186 100644 --- a/include/tinymovr/tinymovr.hpp +++ b/include/tinymovr/tinymovr.hpp @@ -15,15 +15,21 @@ #include #include #include +#include #include -static uint32_t avlos_proto_hash = 3273002564; +static uint32_t avlos_proto_hash = 4118115615; enum errors_flags { ERRORS_NONE = 0, ERRORS_UNDERVOLTAGE = (1 << 0), - ERRORS_DRIVER_FAULT = (1 << 1) + 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 @@ -68,21 +74,42 @@ enum traj_planner_errors_flags 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): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , scheduler(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , controller(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , comms(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , motor(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , encoder(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , traj_planner(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) - , watchdog(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); @@ -92,12 +119,14 @@ class Tinymovr : Node 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 index cd751d9..e4b96a4 100644 --- a/include/tinymovr/traj_planner.hpp +++ b/include/tinymovr/traj_planner.hpp @@ -14,14 +14,20 @@ 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): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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 index 50bf210..e9fb5c0 100644 --- a/include/tinymovr/velocity.hpp +++ b/include/tinymovr/velocity.hpp @@ -14,8 +14,8 @@ class Velocity_ : Node { public: - Velocity_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/include/tinymovr/voltage.hpp b/include/tinymovr/voltage.hpp index 1a4f10d..661bb4e 100644 --- a/include/tinymovr/voltage.hpp +++ b/include/tinymovr/voltage.hpp @@ -14,8 +14,8 @@ class Voltage_ : Node { public: - Voltage_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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 index a2f9693..538eb1e 100644 --- a/include/tinymovr/watchdog.hpp +++ b/include/tinymovr/watchdog.hpp @@ -14,8 +14,8 @@ class Watchdog_ : Node { public: - Watchdog_(uint8_t _can_node_id, send_callback _send_cb, recv_callback _recv_cb, delay_us_callback _delay_us_cb): - Node(_can_node_id, _send_cb, _recv_cb, _delay_us_cb) {}; + 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); diff --git a/src/tinymovr/can.cpp b/src/tinymovr/can.cpp index 028d78d..33b1da0 100644 --- a/src/tinymovr/can.cpp +++ b/src/tinymovr/can.cpp @@ -11,8 +11,8 @@ uint32_t Can_::get_rate(void) { uint32_t value = 0; - this->send(41, this->_data, 0, true); - if (this->recv(41, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(42, this->_data, 0, true); + if (this->recv(42, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ uint32_t Can_::get_rate(void) void Can_::set_rate(uint32_t value) { write_le(value, this->_data); - this->send(41, this->_data, sizeof(uint32_t), false); + this->send(42, this->_data, sizeof(uint32_t), false); } uint32_t Can_::get_id(void) { uint32_t value = 0; - this->send(42, this->_data, 0, true); - if (this->recv(42, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(43, this->_data, 0, true); + if (this->recv(43, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,7 +39,7 @@ uint32_t Can_::get_id(void) void Can_::set_id(uint32_t value) { write_le(value, this->_data); - this->send(42, this->_data, sizeof(uint32_t), false); + this->send(43, this->_data, sizeof(uint32_t), false); } diff --git a/src/tinymovr/controller.cpp b/src/tinymovr/controller.cpp index 54e7c6b..9bd6447 100644 --- a/src/tinymovr/controller.cpp +++ b/src/tinymovr/controller.cpp @@ -11,8 +11,8 @@ uint8_t Controller_::get_state(void) { uint8_t value = 0; - this->send(14, this->_data, 0, true); - if (this->recv(14, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(15, this->_data, 0, true); + if (this->recv(15, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ uint8_t Controller_::get_state(void) void Controller_::set_state(uint8_t value) { write_le(value, this->_data); - this->send(14, this->_data, sizeof(uint8_t), false); + this->send(15, this->_data, sizeof(uint8_t), false); } uint8_t Controller_::get_mode(void) { uint8_t value = 0; - this->send(15, this->_data, 0, true); - if (this->recv(15, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(16, this->_data, 0, true); + if (this->recv(16, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,14 +39,14 @@ uint8_t Controller_::get_mode(void) void Controller_::set_mode(uint8_t value) { write_le(value, this->_data); - this->send(15, this->_data, sizeof(uint8_t), false); + this->send(16, this->_data, sizeof(uint8_t), false); } uint8_t Controller_::get_warnings(void) { uint8_t value = 0; - this->send(16, this->_data, 0, true); - if (this->recv(16, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(17, this->_data, 0, true); + if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -56,8 +56,8 @@ uint8_t Controller_::get_warnings(void) uint8_t Controller_::get_errors(void) { uint8_t value = 0; - this->send(17, this->_data, 0, true); - if (this->recv(17, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(18, this->_data, 0, true); + if (this->recv(18, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -67,27 +67,27 @@ uint8_t Controller_::get_errors(void) void Controller_::calibrate() { - this->send(35, this->_data, 0, true); + this->send(36, this->_data, 0, true); } void Controller_::idle() { - this->send(36, this->_data, 0, true); + this->send(37, this->_data, 0, true); } void Controller_::position_mode() { - this->send(37, this->_data, 0, true); + this->send(38, this->_data, 0, true); } void Controller_::velocity_mode() { - this->send(38, this->_data, 0, true); + this->send(39, this->_data, 0, true); } void Controller_::current_mode() { - this->send(39, this->_data, 0, true); + this->send(40, this->_data, 0, true); } float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint) @@ -98,10 +98,10 @@ float Controller_::set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint) write_le(vel_setpoint, this->_data + data_len); data_len += sizeof(vel_setpoint); - this->send(40, this->_data, data_len, false); + 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), RECV_DELAY_US)) + if (this->recv(17, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } diff --git a/src/tinymovr/current.cpp b/src/tinymovr/current.cpp index 3a72f4b..c7873f8 100644 --- a/src/tinymovr/current.cpp +++ b/src/tinymovr/current.cpp @@ -11,8 +11,8 @@ float Current_::get_Iq_setpoint(void) { float value = 0; - this->send(26, this->_data, 0, true); - if (this->recv(26, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(27, this->_data, 0, true); + if (this->recv(27, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ float Current_::get_Iq_setpoint(void) void Current_::set_Iq_setpoint(float value) { write_le(value, this->_data); - this->send(26, this->_data, sizeof(float), false); + this->send(27, this->_data, sizeof(float), false); } float Current_::get_Id_setpoint(void) { float value = 0; - this->send(27, this->_data, 0, true); - if (this->recv(27, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(28, this->_data, 0, true); + if (this->recv(28, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,8 +39,8 @@ float Current_::get_Id_setpoint(void) float Current_::get_Iq_limit(void) { float value = 0; - this->send(28, this->_data, 0, true); - if (this->recv(28, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(29, this->_data, 0, true); + if (this->recv(29, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -50,14 +50,14 @@ float Current_::get_Iq_limit(void) void Current_::set_Iq_limit(float value) { write_le(value, this->_data); - this->send(28, this->_data, sizeof(float), false); + this->send(29, this->_data, sizeof(float), false); } float Current_::get_Iq_estimate(void) { float value = 0; - this->send(29, this->_data, 0, true); - if (this->recv(29, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(30, this->_data, 0, true); + if (this->recv(30, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -67,8 +67,8 @@ float Current_::get_Iq_estimate(void) float Current_::get_bandwidth(void) { float value = 0; - this->send(30, this->_data, 0, true); - if (this->recv(30, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(31, this->_data, 0, true); + if (this->recv(31, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -78,14 +78,14 @@ float Current_::get_bandwidth(void) void Current_::set_bandwidth(float value) { write_le(value, this->_data); - this->send(30, this->_data, sizeof(float), false); + this->send(31, this->_data, sizeof(float), false); } float Current_::get_Iq_p_gain(void) { float value = 0; - this->send(31, this->_data, 0, true); - if (this->recv(31, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(32, this->_data, 0, true); + if (this->recv(32, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -95,8 +95,8 @@ float Current_::get_Iq_p_gain(void) float Current_::get_max_Ibus_regen(void) { float value = 0; - this->send(32, this->_data, 0, true); - if (this->recv(32, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(33, this->_data, 0, true); + if (this->recv(33, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -106,14 +106,14 @@ float Current_::get_max_Ibus_regen(void) void Current_::set_max_Ibus_regen(float value) { write_le(value, this->_data); - this->send(32, this->_data, sizeof(float), false); + this->send(33, this->_data, sizeof(float), false); } float Current_::get_max_Ibrake(void) { float value = 0; - this->send(33, this->_data, 0, true); - if (this->recv(33, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(34, this->_data, 0, true); + if (this->recv(34, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -123,7 +123,7 @@ float Current_::get_max_Ibrake(void) void Current_::set_max_Ibrake(float value) { write_le(value, this->_data); - this->send(33, this->_data, sizeof(float), false); + this->send(34, this->_data, sizeof(float), false); } diff --git a/src/tinymovr/encoder.cpp b/src/tinymovr/encoder.cpp index d919bf8..9622c48 100644 --- a/src/tinymovr/encoder.cpp +++ b/src/tinymovr/encoder.cpp @@ -11,8 +11,8 @@ float Encoder_::get_position_estimate(void) { float value = 0; - this->send(52, this->_data, 0, true); - if (this->recv(52, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(53, this->_data, 0, true); + if (this->recv(53, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,8 +22,8 @@ float Encoder_::get_position_estimate(void) float Encoder_::get_velocity_estimate(void) { float value = 0; - this->send(53, this->_data, 0, true); - if (this->recv(53, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(54, this->_data, 0, true); + if (this->recv(54, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -33,8 +33,8 @@ float Encoder_::get_velocity_estimate(void) uint8_t Encoder_::get_type(void) { uint8_t value = 0; - this->send(54, this->_data, 0, true); - if (this->recv(54, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(55, this->_data, 0, true); + if (this->recv(55, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -44,14 +44,14 @@ uint8_t Encoder_::get_type(void) void Encoder_::set_type(uint8_t value) { write_le(value, this->_data); - this->send(54, this->_data, sizeof(uint8_t), false); + this->send(55, this->_data, sizeof(uint8_t), false); } float Encoder_::get_bandwidth(void) { float value = 0; - this->send(55, this->_data, 0, true); - if (this->recv(55, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(56, this->_data, 0, true); + if (this->recv(56, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -61,14 +61,14 @@ float Encoder_::get_bandwidth(void) void Encoder_::set_bandwidth(float value) { write_le(value, this->_data); - this->send(55, this->_data, sizeof(float), false); + this->send(56, this->_data, sizeof(float), false); } bool Encoder_::get_calibrated(void) { bool value = 0; - this->send(56, this->_data, 0, true); - if (this->recv(56, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(57, this->_data, 0, true); + if (this->recv(57, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -78,8 +78,8 @@ bool Encoder_::get_calibrated(void) uint8_t Encoder_::get_errors(void) { uint8_t value = 0; - this->send(57, this->_data, 0, true); - if (this->recv(57, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(58, this->_data, 0, true); + if (this->recv(58, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } 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 index db9ff2d..2eb11bc 100644 --- a/src/tinymovr/motor.cpp +++ b/src/tinymovr/motor.cpp @@ -11,8 +11,8 @@ float Motor_::get_R(void) { float value = 0; - this->send(43, this->_data, 0, true); - if (this->recv(43, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(44, this->_data, 0, true); + if (this->recv(44, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ float Motor_::get_R(void) void Motor_::set_R(float value) { write_le(value, this->_data); - this->send(43, this->_data, sizeof(float), false); + this->send(44, this->_data, sizeof(float), false); } float Motor_::get_L(void) { float value = 0; - this->send(44, this->_data, 0, true); - if (this->recv(44, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(45, this->_data, 0, true); + if (this->recv(45, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,14 +39,14 @@ float Motor_::get_L(void) void Motor_::set_L(float value) { write_le(value, this->_data); - this->send(44, this->_data, sizeof(float), false); + this->send(45, this->_data, sizeof(float), false); } uint8_t Motor_::get_pole_pairs(void) { uint8_t value = 0; - this->send(45, this->_data, 0, true); - if (this->recv(45, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(46, this->_data, 0, true); + if (this->recv(46, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -56,14 +56,14 @@ uint8_t Motor_::get_pole_pairs(void) void Motor_::set_pole_pairs(uint8_t value) { write_le(value, this->_data); - this->send(45, this->_data, sizeof(uint8_t), false); + this->send(46, this->_data, sizeof(uint8_t), false); } uint8_t Motor_::get_type(void) { uint8_t value = 0; - this->send(46, this->_data, 0, true); - if (this->recv(46, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(47, this->_data, 0, true); + if (this->recv(47, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -73,14 +73,14 @@ uint8_t Motor_::get_type(void) void Motor_::set_type(uint8_t value) { write_le(value, this->_data); - this->send(46, this->_data, sizeof(uint8_t), false); + this->send(47, this->_data, sizeof(uint8_t), false); } float Motor_::get_offset(void) { float value = 0; - this->send(47, this->_data, 0, true); - if (this->recv(47, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(48, this->_data, 0, true); + if (this->recv(48, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -90,14 +90,14 @@ float Motor_::get_offset(void) void Motor_::set_offset(float value) { write_le(value, this->_data); - this->send(47, this->_data, sizeof(float), false); + this->send(48, this->_data, sizeof(float), false); } int8_t Motor_::get_direction(void) { int8_t value = 0; - this->send(48, this->_data, 0, true); - if (this->recv(48, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(49, this->_data, 0, true); + if (this->recv(49, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -107,14 +107,14 @@ int8_t Motor_::get_direction(void) void Motor_::set_direction(int8_t value) { write_le(value, this->_data); - this->send(48, this->_data, sizeof(int8_t), false); + this->send(49, this->_data, sizeof(int8_t), false); } bool Motor_::get_calibrated(void) { bool value = 0; - this->send(49, this->_data, 0, true); - if (this->recv(49, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(50, this->_data, 0, true); + if (this->recv(50, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -124,8 +124,8 @@ bool Motor_::get_calibrated(void) float Motor_::get_I_cal(void) { float value = 0; - this->send(50, this->_data, 0, true); - if (this->recv(50, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(51, this->_data, 0, true); + if (this->recv(51, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -135,14 +135,14 @@ float Motor_::get_I_cal(void) void Motor_::set_I_cal(float value) { write_le(value, this->_data); - this->send(50, this->_data, sizeof(float), false); + this->send(51, this->_data, sizeof(float), false); } uint8_t Motor_::get_errors(void) { uint8_t value = 0; - this->send(51, this->_data, 0, true); - if (this->recv(51, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(52, this->_data, 0, true); + if (this->recv(52, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } diff --git a/src/tinymovr/position.cpp b/src/tinymovr/position.cpp index 4f52c52..6848657 100644 --- a/src/tinymovr/position.cpp +++ b/src/tinymovr/position.cpp @@ -11,8 +11,8 @@ float Position_::get_setpoint(void) { float value = 0; - this->send(18, this->_data, 0, true); - if (this->recv(18, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(19, this->_data, 0, true); + if (this->recv(19, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ float Position_::get_setpoint(void) void Position_::set_setpoint(float value) { write_le(value, this->_data); - this->send(18, this->_data, sizeof(float), false); + this->send(19, this->_data, sizeof(float), false); } float Position_::get_p_gain(void) { float value = 0; - this->send(19, this->_data, 0, true); - if (this->recv(19, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(20, this->_data, 0, true); + if (this->recv(20, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,7 +39,7 @@ float Position_::get_p_gain(void) void Position_::set_p_gain(float value) { write_le(value, this->_data); - this->send(19, this->_data, sizeof(float), false); + this->send(20, this->_data, sizeof(float), false); } diff --git a/src/tinymovr/scheduler.cpp b/src/tinymovr/scheduler.cpp index 07be947..de9bd30 100644 --- a/src/tinymovr/scheduler.cpp +++ b/src/tinymovr/scheduler.cpp @@ -8,33 +8,11 @@ #include -uint32_t Scheduler_::get_total(void) -{ - uint32_t value = 0; - this->send(11, this->_data, 0, true); - if (this->recv(11, this->_data, &(this->_dlc), RECV_DELAY_US)) - { - read_le(&value, this->_data); - } - return value; -} - -uint32_t Scheduler_::get_busy(void) -{ - uint32_t value = 0; - this->send(12, this->_data, 0, true); - if (this->recv(12, this->_data, &(this->_dlc), RECV_DELAY_US)) - { - read_le(&value, this->_data); - } - return value; -} - uint8_t Scheduler_::get_errors(void) { uint8_t value = 0; - this->send(13, this->_data, 0, true); - if (this->recv(13, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(14, this->_data, 0, true); + if (this->recv(14, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } 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 index 82fe78e..f4e977b 100644 --- a/src/tinymovr/tinymovr.cpp +++ b/src/tinymovr/tinymovr.cpp @@ -10,7 +10,7 @@ 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), RECV_DELAY_US)) + if (this->recv(0, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -20,7 +20,26 @@ 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), RECV_DELAY_US)) + 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); } @@ -29,8 +48,8 @@ uint32_t Tinymovr::get_uid(void) float Tinymovr::get_Vbus(void) { float value = 0; - this->send(2, this->_data, 0, true); - if (this->recv(2, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(4, this->_data, 0, true); + if (this->recv(4, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,8 +58,8 @@ float Tinymovr::get_Vbus(void) float Tinymovr::get_Ibus(void) { float value = 0; - this->send(3, this->_data, 0, true); - if (this->recv(3, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(5, this->_data, 0, true); + if (this->recv(5, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -49,8 +68,8 @@ float Tinymovr::get_Ibus(void) float Tinymovr::get_power(void) { float value = 0; - this->send(4, this->_data, 0, true); - if (this->recv(4, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(6, this->_data, 0, true); + if (this->recv(6, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -59,8 +78,8 @@ float Tinymovr::get_power(void) float Tinymovr::get_temp(void) { float value = 0; - this->send(5, this->_data, 0, true); - if (this->recv(5, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(7, this->_data, 0, true); + if (this->recv(7, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -69,8 +88,8 @@ float Tinymovr::get_temp(void) bool Tinymovr::get_calibrated(void) { bool value = 0; - this->send(6, this->_data, 0, true); - if (this->recv(6, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(8, this->_data, 0, true); + if (this->recv(8, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -79,8 +98,8 @@ bool Tinymovr::get_calibrated(void) uint8_t Tinymovr::get_errors(void) { uint8_t value = 0; - this->send(7, this->_data, 0, true); - if (this->recv(7, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(9, this->_data, 0, true); + if (this->recv(9, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -89,16 +108,21 @@ uint8_t Tinymovr::get_errors(void) void Tinymovr::save_config() { - this->send(8, this->_data, 0, true); + this->send(10, this->_data, 0, true); } void Tinymovr::erase_config() { - this->send(9, this->_data, 0, true); + this->send(11, this->_data, 0, true); } void Tinymovr::reset() { - this->send(10, this->_data, 0, true); + 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 index 604a48d..058ec08 100644 --- a/src/tinymovr/traj_planner.cpp +++ b/src/tinymovr/traj_planner.cpp @@ -11,8 +11,8 @@ float Traj_planner_::get_max_accel(void) { float value = 0; - this->send(58, this->_data, 0, true); - if (this->recv(58, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(59, this->_data, 0, true); + if (this->recv(59, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ float Traj_planner_::get_max_accel(void) void Traj_planner_::set_max_accel(float value) { write_le(value, this->_data); - this->send(58, this->_data, sizeof(float), false); + this->send(59, this->_data, sizeof(float), false); } float Traj_planner_::get_max_decel(void) { float value = 0; - this->send(59, this->_data, 0, true); - if (this->recv(59, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(60, this->_data, 0, true); + if (this->recv(60, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,14 +39,14 @@ float Traj_planner_::get_max_decel(void) void Traj_planner_::set_max_decel(float value) { write_le(value, this->_data); - this->send(59, this->_data, sizeof(float), false); + this->send(60, this->_data, sizeof(float), false); } float Traj_planner_::get_max_vel(void) { float value = 0; - this->send(60, this->_data, 0, true); - if (this->recv(60, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(61, this->_data, 0, true); + if (this->recv(61, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -56,7 +56,58 @@ float Traj_planner_::get_max_vel(void) void Traj_planner_::set_max_vel(float value) { write_le(value, this->_data); - this->send(60, this->_data, sizeof(float), false); + 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); } @@ -66,7 +117,7 @@ void Traj_planner_::move_to(float pos_setpoint) write_le(pos_setpoint, this->_data + data_len); data_len += sizeof(pos_setpoint); - this->send(61, this->_data, data_len, false); + this->send(65, this->_data, data_len, false); } void Traj_planner_::move_to_tlimit(float pos_setpoint) @@ -75,13 +126,13 @@ void Traj_planner_::move_to_tlimit(float pos_setpoint) write_le(pos_setpoint, this->_data + data_len); data_len += sizeof(pos_setpoint); - this->send(62, this->_data, data_len, false); + this->send(66, this->_data, data_len, false); } uint8_t Traj_planner_::get_errors(void) { uint8_t value = 0; - this->send(63, this->_data, 0, true); - if (this->recv(63, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(67, this->_data, 0, true); + if (this->recv(67, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } diff --git a/src/tinymovr/velocity.cpp b/src/tinymovr/velocity.cpp index 901b6fe..318995e 100644 --- a/src/tinymovr/velocity.cpp +++ b/src/tinymovr/velocity.cpp @@ -11,8 +11,8 @@ float Velocity_::get_setpoint(void) { float value = 0; - this->send(20, this->_data, 0, true); - if (this->recv(20, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(21, this->_data, 0, true); + if (this->recv(21, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ float Velocity_::get_setpoint(void) void Velocity_::set_setpoint(float value) { write_le(value, this->_data); - this->send(20, this->_data, sizeof(float), false); + this->send(21, this->_data, sizeof(float), false); } float Velocity_::get_limit(void) { float value = 0; - this->send(21, this->_data, 0, true); - if (this->recv(21, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(22, this->_data, 0, true); + if (this->recv(22, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,14 +39,14 @@ float Velocity_::get_limit(void) void Velocity_::set_limit(float value) { write_le(value, this->_data); - this->send(21, this->_data, sizeof(float), false); + this->send(22, this->_data, sizeof(float), false); } float Velocity_::get_p_gain(void) { float value = 0; - this->send(22, this->_data, 0, true); - if (this->recv(22, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(23, this->_data, 0, true); + if (this->recv(23, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -56,14 +56,14 @@ float Velocity_::get_p_gain(void) void Velocity_::set_p_gain(float value) { write_le(value, this->_data); - this->send(22, this->_data, sizeof(float), false); + this->send(23, this->_data, sizeof(float), false); } float Velocity_::get_i_gain(void) { float value = 0; - this->send(23, this->_data, 0, true); - if (this->recv(23, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(24, this->_data, 0, true); + if (this->recv(24, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -73,14 +73,14 @@ float Velocity_::get_i_gain(void) void Velocity_::set_i_gain(float value) { write_le(value, this->_data); - this->send(23, this->_data, sizeof(float), false); + this->send(24, this->_data, sizeof(float), false); } float Velocity_::get_deadband(void) { float value = 0; - this->send(24, this->_data, 0, true); - if (this->recv(24, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(25, this->_data, 0, true); + if (this->recv(25, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -90,14 +90,14 @@ float Velocity_::get_deadband(void) void Velocity_::set_deadband(float value) { write_le(value, this->_data); - this->send(24, this->_data, sizeof(float), false); + this->send(25, this->_data, sizeof(float), false); } float Velocity_::get_increment(void) { float value = 0; - this->send(25, this->_data, 0, true); - if (this->recv(25, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(26, this->_data, 0, true); + if (this->recv(26, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -107,7 +107,7 @@ float Velocity_::get_increment(void) void Velocity_::set_increment(float value) { write_le(value, this->_data); - this->send(25, this->_data, sizeof(float), false); + this->send(26, this->_data, sizeof(float), false); } diff --git a/src/tinymovr/voltage.cpp b/src/tinymovr/voltage.cpp index 151fb16..754640b 100644 --- a/src/tinymovr/voltage.cpp +++ b/src/tinymovr/voltage.cpp @@ -11,8 +11,8 @@ float Voltage_::get_Vq_setpoint(void) { float value = 0; - this->send(34, this->_data, 0, true); - if (this->recv(34, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(35, this->_data, 0, true); + if (this->recv(35, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } diff --git a/src/tinymovr/watchdog.cpp b/src/tinymovr/watchdog.cpp index f400346..35548b2 100644 --- a/src/tinymovr/watchdog.cpp +++ b/src/tinymovr/watchdog.cpp @@ -11,8 +11,8 @@ bool Watchdog_::get_enabled(void) { bool value = 0; - this->send(64, this->_data, 0, true); - if (this->recv(64, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(76, this->_data, 0, true); + if (this->recv(76, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -22,14 +22,14 @@ bool Watchdog_::get_enabled(void) void Watchdog_::set_enabled(bool value) { write_le(value, this->_data); - this->send(64, this->_data, sizeof(bool), false); + this->send(76, this->_data, sizeof(bool), false); } bool Watchdog_::get_triggered(void) { bool value = 0; - this->send(65, this->_data, 0, true); - if (this->recv(65, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(77, this->_data, 0, true); + if (this->recv(77, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -39,8 +39,8 @@ bool Watchdog_::get_triggered(void) float Watchdog_::get_timeout(void) { float value = 0; - this->send(66, this->_data, 0, true); - if (this->recv(66, this->_data, &(this->_dlc), RECV_DELAY_US)) + this->send(78, this->_data, 0, true); + if (this->recv(78, this->_data, &(this->_dlc), this->delay_us_value)) { read_le(&value, this->_data); } @@ -50,7 +50,7 @@ float Watchdog_::get_timeout(void) void Watchdog_::set_timeout(float value) { write_le(value, this->_data); - this->send(66, this->_data, sizeof(float), false); + this->send(78, this->_data, sizeof(float), false); } From aeda05d3bf3240b06c52cc8229fa4ee16722e045 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Fri, 15 Sep 2023 00:06:17 +0300 Subject: [PATCH 50/61] add delay_us param --- config/hardware.yaml | 1 + src/tm_joint_iface.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/config/hardware.yaml b/config/hardware.yaml index a4972a3..024e692 100644 --- a/config/hardware.yaml +++ b/config/hardware.yaml @@ -4,6 +4,7 @@ tinymovr_joint_iface: # hardware ID of the actuator id: 2 offset: 3.14159265359 + delay_us: 100 # offset to be added, in radians, to the position of an actuator #max-speed: 4.0 # in rad command_interface: velocity \ No newline at end of file diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 39a8ac0..df7eba8 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -95,11 +95,13 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) ROS_DEBUG_STREAM("servo: " << (std::string)(it->first)); 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_DEBUG_STREAM("\tid: " << (int)id); - servos.push_back(Tinymovr(id, &send_cb, &recv_cb, &delay_us_cb)); + servos.push_back(Tinymovr(id, &send_cb, &recv_cb, &delay_us_cb, delay_us)); servo_modes.push_back(servos_param[it->first]["command_interface"]); } else From ba7ddd56e97e4a63779084687579334e4c42a413 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sat, 16 Sep 2023 03:57:05 +0300 Subject: [PATCH 51/61] add missing import --- include/tinymovr/helpers.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/tinymovr/helpers.hpp b/include/tinymovr/helpers.hpp index e69a122..4428625 100644 --- a/include/tinymovr/helpers.hpp +++ b/include/tinymovr/helpers.hpp @@ -14,6 +14,7 @@ #else #include #include +#include #endif #if defined ARDUINO From a355e43a5aff626bae190d7c441cc4f8b6b104de Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Sat, 16 Sep 2023 14:23:28 +0300 Subject: [PATCH 52/61] consolidate can interface --- CMakeLists.txt | 2 +- config/hardware.yaml | 2 +- include/tinymovr_can.hpp | 22 ----------- include/tm_joint_iface.hpp | 1 - src/socketcan_cpp.cpp | 20 ++++------ src/tinymovr_can.cpp | 81 -------------------------------------- src/tm_joint_iface.cpp | 76 +++++++++++++++++++++++++++-------- 7 files changed, 68 insertions(+), 136 deletions(-) delete mode 100644 include/tinymovr_can.hpp delete mode 100644 src/tinymovr_can.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 67c2708..271b2c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -148,7 +148,7 @@ set(TINYMOVR_SOURCES # src/${PROJECT_NAME}/tinymovr_ros.cpp # ) -add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp.cpp src/tinymovr_can.cpp src/tm_joint_iface.cpp) +add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/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 diff --git a/config/hardware.yaml b/config/hardware.yaml index 024e692..582583a 100644 --- a/config/hardware.yaml +++ b/config/hardware.yaml @@ -2,7 +2,7 @@ tinymovr_joint_iface: joints: arm_1: # hardware ID of the actuator - id: 2 + id: 1 offset: 3.14159265359 delay_us: 100 # offset to be added, in radians, to the position of an actuator diff --git a/include/tinymovr_can.hpp b/include/tinymovr_can.hpp deleted file mode 100644 index 12a92c3..0000000 --- a/include/tinymovr_can.hpp +++ /dev/null @@ -1,22 +0,0 @@ - -#pragma once - -#include "socketcan_cpp/socketcan_cpp.hpp" -#include - -namespace tinymovr_ros -{ - -class TinymovrCAN -{ -public: - void init(); - bool read_frame(uint32_t *arbitration_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); - bool write_frame(uint32_t arbitration_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/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index ec00d5b..5b1f7ab 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -10,7 +10,6 @@ #include #include -#include #include using namespace std; diff --git a/src/socketcan_cpp.cpp b/src/socketcan_cpp.cpp index d8b8596..01771c8 100644 --- a/src/socketcan_cpp.cpp +++ b/src/socketcan_cpp.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #ifdef HAVE_SOCKETCAN_HEADERS #include @@ -50,12 +49,10 @@ 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) { - ROS_INFO("Opening SocketCAN on interface: %s", can_interface.c_str()); m_interface = can_interface; m_socket_mode = mode; m_read_timeout_ms = read_timeout_ms; @@ -130,7 +127,6 @@ namespace scpp perror("bind"); return STATUS_BIND_ERROR; } - ROS_INFO("SocketCAN opened successfully on interface: %s", can_interface.c_str()); struct can_filter rfilter[1]; rfilter[0].can_id = 0x00000700; rfilter[0].can_mask = 0x1FFFFF00; @@ -143,14 +139,17 @@ namespace scpp #endif return STATUS_OK; } + SocketCanStatus SocketCan::write(const CanFrame & msg) { - ROS_INFO("Writing frame with ID: %d", msg.id); #ifdef HAVE_SOCKETCAN_HEADERS struct canfd_frame frame; 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); @@ -165,15 +164,14 @@ namespace scpp perror("write"); return STATUS_WRITE_ERROR; } - ROS_INFO("Frame with ID: %d written successfully.", msg.id); #else printf("Your operating system does not support socket can! \n"); #endif return STATUS_OK; } + SocketCanStatus SocketCan::read(CanFrame & msg) { - ROS_INFO("Reading frame..."); #ifdef HAVE_SOCKETCAN_HEADERS struct canfd_frame frame; @@ -181,11 +179,9 @@ namespace scpp auto num_bytes = ::read(m_socket, &frame, CANFD_MTU); if (num_bytes != CAN_MTU && num_bytes != CANFD_MTU) { - ROS_ERROR("Error reading frame."); //perror("Can read error"); return STATUS_READ_ERROR; } - ROS_INFO("Read frame with ID: %d successfully.", msg.id); msg.id = frame.can_id; msg.len = frame.len; msg.flags = frame.flags; @@ -197,11 +193,9 @@ namespace scpp } SocketCanStatus SocketCan::close() { - ROS_INFO("Closing SocketCAN on interface: %s", m_interface.c_str()); #ifdef HAVE_SOCKETCAN_HEADERS ::close(m_socket); #endif - ROS_INFO("SocketCAN closed successfully."); return STATUS_OK; } const std::string & SocketCan::interfaceName() const diff --git a/src/tinymovr_can.cpp b/src/tinymovr_can.cpp deleted file mode 100644 index df25023..0000000 --- a/src/tinymovr_can.cpp +++ /dev/null @@ -1,81 +0,0 @@ - -#include -#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* can_id, uint8_t* data, uint8_t* len) -{ - ROS_INFO("in read frame"); - scpp::CanFrame fr; - scpp::SocketCanStatus read_status = socket_can.read(fr); - if (read_status == scpp::STATUS_OK) - { - *can_id = fr.id; - *len = fr.len; - std::copy(fr.data, fr.data + fr.len, data); - ROS_DEBUG("Successfully read a CAN frame"); - return true; - } - else - { - ROS_WARN("Failed to read a CAN frame. SocketCanStatus: %d", static_cast(read_status)); - switch(read_status) - { - case scpp::STATUS_READ_ERROR: - ROS_ERROR("SocketCan read error!"); - break; - // Removed STATUS_TIMEOUT case - default: - ROS_ERROR("Unknown SocketCan error!"); - break; - } - return false; - } -} - - - -bool TinymovrCAN::write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len) -{ - ROS_INFO("in write frame1"); - return write_frame(make_arbitration_id(node_id, ep_id), data, data_len); -} - -bool TinymovrCAN::write_frame(uint32_t arbitration_id, const uint8_t *data, uint8_t data_len) -{ - ROS_INFO("in write frame2"); - scpp::CanFrame cf_to_write; - - cf_to_write.id = arbitration_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_joint_iface.cpp b/src/tm_joint_iface.cpp index df7eba8..31666f9 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -7,6 +7,9 @@ #include #include +#include + +#include "socketcan_cpp/socketcan_cpp.hpp" #include using namespace std; @@ -15,7 +18,7 @@ namespace tinymovr_ros { -TinymovrCAN tmcan; +scpp::SocketCan socket_can; // --------------------------------------------------------------- /* @@ -25,19 +28,28 @@ TinymovrCAN tmcan; * * arbitration_id: the frame arbitration id * data: pointer to the data array to be transmitted - * data_size: the size of transmitted data + * data_length: the size of transmitted data * rtr: if the frame is of request transmit type (RTR) */ -void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr) +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); - - if (!tmcan.write_frame(arbitration_id, data, data_size)) + + scpp::CanFrame cf_to_write; + + cf_to_write.id = arbitration_id; + cf_to_write.len = data_length; + for (int i = 0; i < data_length; ++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) { throw std::runtime_error("CAN write error"); } - - ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << arbitration_id << " written successfully."); + else + { + ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << arbitration_id << " written successfully."); + } } /* @@ -47,19 +59,36 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr * * arbitration_id: the frame arbitration id pointer * data: pointer to the data array to be received - * data_size: pointer to the variable that will hold the size of received data + * data_length: pointer to the variable that will hold the size of received data */ -bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_size) +bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_length) { ROS_DEBUG_STREAM("Attempting to read CAN frame..."); - if (!tmcan.read_frame(arbitration_id, data, data_size)) + scpp::CanFrame fr; + scpp::SocketCanStatus read_status = socket_can.read(fr); + if (read_status == scpp::STATUS_OK) { - throw std::runtime_error("CAN read error"); + *arbitration_id = fr.id; + *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 + { + switch(read_status) + { + case scpp::STATUS_READ_ERROR: + throw std::runtime_error("SocketCAN read error"); + break; + // Removed STATUS_TIMEOUT case + default: + throw std::runtime_error("Unknown SocketCAN error"); + break; + } + return false; } - - ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << *arbitration_id << " read successfully."); - return true; } /* @@ -92,7 +121,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) try { for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = servos_param.begin(); it != servos_param.end(); ++it) { - ROS_DEBUG_STREAM("servo: " << (std::string)(it->first)); + ROS_INFO_STREAM("servo: " << (std::string)(it->first)); id_t id; int delay_us; @@ -100,7 +129,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { id = static_cast(servos_param[it->first]["id"]); delay_us = static_cast(servos_param[it->first]["delay_us"]); - ROS_DEBUG_STREAM("\tid: " << (int)id); + ROS_INFO_STREAM("\tid: " << (int)id); servos.push_back(Tinymovr(id, &send_cb, &recv_cb, &delay_us_cb, delay_us)); servo_modes.push_back(servos_param[it->first]["command_interface"]); } @@ -138,15 +167,28 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) joint_velocity_state.resize(num_joints, 0.0); joint_effort_state.resize(num_joints, 0.0); - tmcan.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); + } // initialize servos with correct mode for (int i=0; i Date: Sun, 17 Sep 2023 01:49:33 +0300 Subject: [PATCH 53/61] working comms --- CMakeLists.txt | 2 +- config/hardware.yaml | 2 +- src/{ => socketcan_cpp}/socketcan_cpp.cpp | 8 ++-- src/tm_joint_iface.cpp | 57 ++++++++++++++++------- 4 files changed, 47 insertions(+), 22 deletions(-) rename src/{ => socketcan_cpp}/socketcan_cpp.cpp (98%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 271b2c5..0d3fcb9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -148,7 +148,7 @@ set(TINYMOVR_SOURCES # src/${PROJECT_NAME}/tinymovr_ros.cpp # ) -add_library(${PROJECT_NAME}_joint_iface ${TINYMOVR_SOURCES} src/socketcan_cpp.cpp src/tm_joint_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 diff --git a/config/hardware.yaml b/config/hardware.yaml index 582583a..a33d9b7 100644 --- a/config/hardware.yaml +++ b/config/hardware.yaml @@ -4,7 +4,7 @@ tinymovr_joint_iface: # hardware ID of the actuator id: 1 offset: 3.14159265359 - delay_us: 100 + delay_us: 1000 # offset to be added, in radians, to the position of an actuator #max-speed: 4.0 # in rad command_interface: velocity \ No newline at end of file diff --git a/src/socketcan_cpp.cpp b/src/socketcan_cpp/socketcan_cpp.cpp similarity index 98% rename from src/socketcan_cpp.cpp rename to src/socketcan_cpp/socketcan_cpp.cpp index 01771c8..a62b5ad 100644 --- a/src/socketcan_cpp.cpp +++ b/src/socketcan_cpp/socketcan_cpp.cpp @@ -128,7 +128,7 @@ namespace scpp return STATUS_BIND_ERROR; } struct can_filter rfilter[1]; - rfilter[0].can_id = 0x00000700; + 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"); @@ -174,12 +174,10 @@ namespace scpp { #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; @@ -191,6 +189,7 @@ namespace scpp #endif return STATUS_OK; } + SocketCanStatus SocketCan::close() { #ifdef HAVE_SOCKETCAN_HEADERS @@ -202,6 +201,7 @@ namespace scpp { return m_interface; } + SocketCan::~SocketCan() { close(); diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 31666f9..e885fa3 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -20,6 +20,31 @@ 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"; + } +} + // --------------------------------------------------------------- /* * Function: send_cb @@ -38,13 +63,16 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_length, bool r 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_sc_status = socket_can.write(cf_to_write); - if (write_sc_status != scpp::STATUS_OK) + auto write_status = socket_can.write(cf_to_write); + if (write_status != scpp::STATUS_OK) { - throw std::runtime_error("CAN write error"); + throw std::runtime_error(SocketCanErrorToString(write_status)); } else { @@ -69,7 +97,7 @@ bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_length) scpp::SocketCanStatus read_status = socket_can.read(fr); if (read_status == scpp::STATUS_OK) { - *arbitration_id = fr.id; + *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."); @@ -77,16 +105,7 @@ bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_length) } else { - switch(read_status) - { - case scpp::STATUS_READ_ERROR: - throw std::runtime_error("SocketCAN read error"); - break; - // Removed STATUS_TIMEOUT case - default: - throw std::runtime_error("Unknown SocketCAN error"); - break; - } + throw std::runtime_error(SocketCanErrorToString(read_status)); return false; } } @@ -100,7 +119,7 @@ bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_length) */ void delay_us_cb(uint32_t us) { - ros::Duration(us * 1e-6).sleep(); + ros::Duration(us * 1e-6).sleep(); } // --------------------------------------------------------------- @@ -129,7 +148,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { id = static_cast(servos_param[it->first]["id"]); delay_us = static_cast(servos_param[it->first]["delay_us"]); - ROS_INFO_STREAM("\tid: " << (int)id); + 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"]); } @@ -181,8 +200,14 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) // initialize servos with correct mode for (int i=0; i Date: Tue, 19 Sep 2023 21:56:50 +0300 Subject: [PATCH 54/61] everything working, incl shutdown --- config/hardware.yaml | 2 +- include/tm_joint_iface.hpp | 1 + package.xml | 5 +++++ src/tm_joint_iface.cpp | 32 ++++++++++++++++++++++++-------- src/tm_joint_iface_node.cpp | 4 ++++ 5 files changed, 35 insertions(+), 9 deletions(-) diff --git a/config/hardware.yaml b/config/hardware.yaml index a33d9b7..becfecc 100644 --- a/config/hardware.yaml +++ b/config/hardware.yaml @@ -2,7 +2,7 @@ tinymovr_joint_iface: joints: arm_1: # hardware ID of the actuator - id: 1 + id: 2 offset: 3.14159265359 delay_us: 1000 # offset to be added, in radians, to the position of an actuator diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index 5b1f7ab..efde441 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -25,6 +25,7 @@ class TinymovrJoint : public hardware_interface::RobotHW 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_; diff --git a/package.xml b/package.xml index 3274eb6..5da5719 100644 --- a/package.xml +++ b/package.xml @@ -49,6 +49,11 @@ catkin + + velocity_controllers + position_controllers + effort_controllers + controller_interface controller_manager geometry_msgs diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index e885fa3..7f80549 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -140,7 +140,9 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) try { for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = servos_param.begin(); it != servos_param.end(); ++it) { - ROS_INFO_STREAM("servo: " << (std::string)(it->first)); + 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; @@ -186,7 +188,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) joint_velocity_state.resize(num_joints, 0.0); joint_effort_state.resize(num_joints, 0.0); - auto status = socket_can.open("can0"); + const scpp::SocketCanStatus status = socket_can.open("can0"); if (scpp::STATUS_OK == status) { ROS_INFO("Socketcan opened successfully"); @@ -210,13 +212,15 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) { ROS_INFO("Setting state"); servos[i].controller.set_state(2); - ROS_INFO("Setting mode"); - servos[i].controller.set_mode(_str2mode(servo_modes[i])); + const uint8_t mode = _str2mode(servo_modes[i]); + ROS_INFO("Setting mode to %u", mode); + servos[i].controller.set_mode(mode); ros::Duration(0.001).sleep(); ROS_INFO("Asserting state and mode"); - ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == 2)); + ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == 1)); } + ROS_INFO("Registering Interfaces"); // register state interfaces for (int i=0; i Date: Wed, 20 Sep 2023 00:23:23 +0300 Subject: [PATCH 55/61] add basic documentation --- README.md | 67 +++++++++++++++++--- src/tm_joint_iface.cpp | 135 ++++++++++++++++++++++++++++------------- 2 files changed, 153 insertions(+), 49 deletions(-) diff --git a/README.md b/README.md index dbc4534..0b914d7 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,68 @@ __Tinymovr ROS Hardware Interface__ -ROS Hardware interface for working with Tinymovr motor controllers. +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. -__Useful Resources__ +## 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. -These are not directly related to this repo, but were useful when trying to install ROS in a Raspbeery Pi +## Prerequisites -Installing Catkin Tools: https://catkin-tools.readthedocs.io/en/latest/installing.html +- 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. -Installing liburdfdom: https://www.howtoinstall.me/ubuntu/18-04/liburdfdom-headers-dev/ +## Installation -Socketcan library: https://github.com/siposcsaba89/socketcan-cpp +1. Navigate to your catkin workspace's source folder: -Issue with libudev: https://stackoverflow.com/questions/17808084/fatal-error-libudev-h-no-such-file-or-directory +```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 +``` + +## Usage + +1. Start the `tinymovr_joint_iface` node: + +```bash +roslaunch tinymovr_joint_iface tinymovr_joint_iface.launch +``` + +2. Publish commands or subscribe to the relevant ROS topics for joint control and state information. + +## Configuration + +To customize the behavior of the `tinymovr_joint_iface`, 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_joint_iface` 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/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 7f80549..d05bd42 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -17,7 +17,6 @@ using namespace std; namespace tinymovr_ros { - scpp::SocketCan socket_can; const char* SocketCanErrorToString(scpp::SocketCanStatus status) { @@ -45,16 +44,20 @@ const char* SocketCanErrorToString(scpp::SocketCanStatus status) { } } -// --------------------------------------------------------------- -/* - * Function: send_cb - * -------------------- - * Is called to send a CAN frame - * - * arbitration_id: the frame arbitration id - * data: pointer to the data array to be transmitted - * data_length: the size of transmitted data - * rtr: if the frame is of request transmit type (RTR) +/** + * @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) { @@ -80,14 +83,21 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_length, bool r } } -/* - * Function: recv_cb - * -------------------- - * Is called to receive a CAN frame - * - * arbitration_id: the frame arbitration id pointer - * data: pointer to the data array to be received - * data_length: pointer to the variable that will hold the size of received data +/** + * @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) { @@ -110,23 +120,41 @@ bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_length) } } -/* - * Function: delay_us_cb - * -------------------- - * Is called to perform a delay - * - * us: the microseconds to wait for +/** + * @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) { @@ -256,24 +284,36 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) return true; } - /** Convert a string to an operating mode - @param mode_string name of the operating mode (current or effort, velocity or position) - @return mode index corresponding to the index of the mode - **/ - uint8_t TinymovrJoint::_str2mode(std::string& mode_string) - { - if ("current" == mode_string || "effort" == mode_string) - return 0; - if ("velocity" == mode_string) - return 1; - else if ("position" == mode_string) - return 2; - else { - ROS_ERROR_STREAM("The command mode " << mode_string << " is not available"); - return 0; - } +/** + * @brief Convert a string to an operating mode + * + * @param mode_string name of the operating mode (current or effort, velocity or position) + * @return mode index corresponding to the index of the mode + */ +uint8_t TinymovrJoint::_str2mode(std::string& mode_string) +{ + if ("current" == mode_string || "effort" == mode_string) + return 0; + if ("velocity" == mode_string) + return 1; + else if ("position" == mode_string) + return 2; + else { + ROS_ERROR_STREAM("The command mode " << mode_string << " is not available"); + return 0; } +} +/** + * @brief Reads the state of each servo in the TinymovrJoint. + * + * This function iterates over each servo connected to the TinymovrJoint + * and fetches its current position, velocity, and effort (current) state. + * In case of any communication or other exceptions, an error is logged. + * + * @param time Current ROS time. + * @param period Time since the last call to this function. + */ void TinymovrJoint::read(const ros::Time& time, const ros::Duration& period) { try { @@ -290,6 +330,17 @@ void TinymovrJoint::read(const ros::Time& time, const ros::Duration& period) } } +/** + * @brief Writes command values to each servo in the TinymovrJoint. + * + * This function iterates over each servo connected to the TinymovrJoint + * and sets the position, velocity, and effort (current) setpoints + * as per the current command values. + * In case of any communication or other exceptions, an error is logged. + * + * @param time Current ROS time. + * @param period Time since the last call to this function. + */ void TinymovrJoint::write(const ros::Time& time, const ros::Duration& period) { try { From 2d5adb2b4ae18288964f3bf0c8bb2e4b90c1dcb2 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Wed, 20 Sep 2023 00:23:34 +0300 Subject: [PATCH 56/61] add better messages --- config/hardware.yaml | 12 ++++++++++-- src/tm_joint_iface.cpp | 10 ++++++++-- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/config/hardware.yaml b/config/hardware.yaml index becfecc..ac0c6f6 100644 --- a/config/hardware.yaml +++ b/config/hardware.yaml @@ -1,10 +1,18 @@ tinymovr_joint_iface: joints: - arm_1: + arm_2: # hardware ID of the actuator id: 2 offset: 3.14159265359 - delay_us: 1000 + delay_us: 200 + # offset to be added, in radians, to the position of an actuator + #max-speed: 4.0 # in rad + command_interface: velocity + arm_1: + # hardware ID of the actuator + id: 1 + offset: 3.14159265359 + delay_us: 200 # offset to be added, in radians, to the position of an actuator #max-speed: 4.0 # in rad command_interface: velocity \ No newline at end of file diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index d05bd42..6937467 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -228,12 +228,18 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) } // initialize servos with correct mode + ROS_INFO("Asserting spec compatibility"); for (int i=0; i Date: Wed, 20 Sep 2023 21:42:16 +0300 Subject: [PATCH 57/61] use scaling factor --- config/diff_drive_config.yaml | 25 +++++++++++++++++++++++++ config/hardware.yaml | 19 ++++++++++--------- config/velocity_controller.yaml | 4 ---- include/tm_joint_iface.hpp | 1 + launch/tinymovr_joint_iface_node.launch | 17 +++++++++++------ package.xml | 1 + src/tm_joint_iface.cpp | 10 ++++++---- 7 files changed, 54 insertions(+), 23 deletions(-) create mode 100644 config/diff_drive_config.yaml delete mode 100644 config/velocity_controller.yaml diff --git a/config/diff_drive_config.yaml b/config/diff_drive_config.yaml new file mode 100644 index 0000000..4869623 --- /dev/null +++ b/config/diff_drive_config.yaml @@ -0,0 +1,25 @@ +diff_drive_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: 'arm_1' + right_wheel: 'arm_2' + publish_rate: 50.0 + required_drive_mode: 'velocity' + wheel_separation: 0.3 + wheel_radius: 0.1 + velocity_rolling_window_size: 10 + + # Assuming you have velocity command interface, if not adjust accordingly + left_wheel_name: arm_1 + right_wheel_name: arm_2 + velocity_rolling_window_size: 10 + + publish_wheel_joint_controller_state: true + + # Make sure to match these with your hardware's limits + max_wheel_velocity: 0.5 # in m/s, adjust as necessary + min_wheel_velocity: -0.5 # in m/s, adjust as necessary + + # Hardware interface parameters + joints: + - arm_1 + - arm_2 diff --git a/config/hardware.yaml b/config/hardware.yaml index ac0c6f6..a292dd9 100644 --- a/config/hardware.yaml +++ b/config/hardware.yaml @@ -1,18 +1,19 @@ tinymovr_joint_iface: joints: - arm_2: + wheel_2: # hardware ID of the actuator id: 2 - offset: 3.14159265359 + offset: 3.14159265359 # in rad delay_us: 200 - # offset to be added, in radians, to the position of an actuator - #max-speed: 4.0 # in rad + #max-speed: 4.0 # in rad/s + rads_to_ticks: 14.323944878 # how many ticks in a rad command_interface: velocity - arm_1: + wheel_1: # hardware ID of the actuator id: 1 - offset: 3.14159265359 + offset: 3.14159265359 # in rad delay_us: 200 - # offset to be added, in radians, to the position of an actuator - #max-speed: 4.0 # in rad - command_interface: velocity \ No newline at end of file + #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/velocity_controller.yaml b/config/velocity_controller.yaml deleted file mode 100644 index 8fb8f98..0000000 --- a/config/velocity_controller.yaml +++ /dev/null @@ -1,4 +0,0 @@ -arm_controller: - type: "velocity_controllers/JointVelocityController" - joint: arm_1 - pid: {p: 1.0, i: 0.0, d: 0.0} \ No newline at end of file diff --git a/include/tm_joint_iface.hpp b/include/tm_joint_iface.hpp index efde441..6c6d18b 100644 --- a/include/tm_joint_iface.hpp +++ b/include/tm_joint_iface.hpp @@ -38,6 +38,7 @@ class TinymovrJoint : public hardware_interface::RobotHW 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; diff --git a/launch/tinymovr_joint_iface_node.launch b/launch/tinymovr_joint_iface_node.launch index 317a05e..e502d3c 100644 --- a/launch/tinymovr_joint_iface_node.launch +++ b/launch/tinymovr_joint_iface_node.launch @@ -1,8 +1,13 @@ - - - - - \ No newline at end of file + + + + + + + + + + + diff --git a/package.xml b/package.xml index 5da5719..23d2f8c 100644 --- a/package.xml +++ b/package.xml @@ -86,6 +86,7 @@ std_msgs tf sensor_msgs + diff_drive_controller diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 6937467..6fb5db4 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -181,6 +181,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) 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 { @@ -325,8 +326,9 @@ void TinymovrJoint::read(const ros::Time& time, const ros::Duration& period) try { for (int i=0; i Date: Wed, 20 Sep 2023 21:48:06 +0300 Subject: [PATCH 58/61] rename launchfile --- ..._joint_iface_node.launch => tinymovr_diffbot_demo_node.launch} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename launch/{tinymovr_joint_iface_node.launch => tinymovr_diffbot_demo_node.launch} (100%) diff --git a/launch/tinymovr_joint_iface_node.launch b/launch/tinymovr_diffbot_demo_node.launch similarity index 100% rename from launch/tinymovr_joint_iface_node.launch rename to launch/tinymovr_diffbot_demo_node.launch From 2b4b5d3cae94690a7664a20e1b8c0bb6a6876479 Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Wed, 20 Sep 2023 23:28:47 +0300 Subject: [PATCH 59/61] fix configuration --- config/diff_drive_config.yaml | 29 ++++++------------------ launch/tinymovr_diffbot_demo_node.launch | 5 +--- 2 files changed, 8 insertions(+), 26 deletions(-) diff --git a/config/diff_drive_config.yaml b/config/diff_drive_config.yaml index 4869623..e722020 100644 --- a/config/diff_drive_config.yaml +++ b/config/diff_drive_config.yaml @@ -1,25 +1,10 @@ diff_drive_controller: type: "diff_drive_controller/DiffDriveController" - left_wheel: 'arm_1' - right_wheel: 'arm_2' + 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 - required_drive_mode: 'velocity' - wheel_separation: 0.3 - wheel_radius: 0.1 - velocity_rolling_window_size: 10 - - # Assuming you have velocity command interface, if not adjust accordingly - left_wheel_name: arm_1 - right_wheel_name: arm_2 - velocity_rolling_window_size: 10 - - publish_wheel_joint_controller_state: true - - # Make sure to match these with your hardware's limits - max_wheel_velocity: 0.5 # in m/s, adjust as necessary - min_wheel_velocity: -0.5 # in m/s, adjust as necessary - - # Hardware interface parameters - joints: - - arm_1 - - arm_2 + wheel_separation: 0.4 + wheel_radius: 0.12 + cmd_vel_timeout: 0.5 diff --git a/launch/tinymovr_diffbot_demo_node.launch b/launch/tinymovr_diffbot_demo_node.launch index e502d3c..d39ab7c 100644 --- a/launch/tinymovr_diffbot_demo_node.launch +++ b/launch/tinymovr_diffbot_demo_node.launch @@ -6,8 +6,5 @@ - - - - + From e3f0679628c8472c07429527db804bc83ea7868f Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Wed, 20 Sep 2023 23:29:01 +0300 Subject: [PATCH 60/61] compare with actual mode --- src/tm_joint_iface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tm_joint_iface.cpp b/src/tm_joint_iface.cpp index 6fb5db4..4902874 100644 --- a/src/tm_joint_iface.cpp +++ b/src/tm_joint_iface.cpp @@ -252,7 +252,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) servos[i].controller.set_mode(mode); ros::Duration(0.001).sleep(); ROS_INFO("Asserting state and mode"); - ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == 1)); + ROS_ASSERT((servos[i].controller.get_state() == 2) && (servos[i].controller.get_mode() == mode)); } ROS_INFO("Registering Interfaces"); From 7541a50c6996ec9c654253b49a39ac5dd830e8ab Mon Sep 17 00:00:00 2001 From: Yannis Chatzikonstantinou Date: Wed, 20 Sep 2023 23:34:22 +0300 Subject: [PATCH 61/61] update readme --- README.md | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 0b914d7..d7181e3 100644 --- a/README.md +++ b/README.md @@ -41,19 +41,27 @@ catkin_make source devel/setup.bash ``` -## Usage +## Run the Diffbot demo! -1. Start the `tinymovr_joint_iface` node: +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_joint_iface tinymovr_joint_iface.launch +roslaunch tinymovr_ros tinymovr_diffbot_demo_node.launch ``` -2. Publish commands or subscribe to the relevant ROS topics for joint control and state information. +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_joint_iface`, 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. +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 @@ -61,7 +69,7 @@ Further details about the API and individual functions can be found in the gener ## Contributing -Contributions to improve and expand the functionality of `tinymovr_joint_iface` are welcome! Please open an issue or submit a pull request on the GitHub repository. +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