├── README.md ├── docs └── infra.png ├── robot_control_system ├── CMakeLists.txt ├── config │ └── arduino_actuator_control.yaml ├── include │ └── robot_control_system │ │ ├── arduino_actuator_interface.hpp │ │ ├── serial_port.hpp │ │ └── visibility_control.h ├── launch │ └── start_actuator_demo.launch.py ├── package.xml ├── robot_control_system.xml ├── rviz │ └── control.rviz ├── src │ ├── arduino_actuator_interface.cpp │ └── serial_port.cpp └── urdf │ └── arduino_actuator_control.xacro ├── robot_control_system_tester ├── CMakeLists.txt ├── include │ └── robot_control_system_tester │ │ └── robot_control_commander.hpp ├── package.xml └── src │ └── robot_control_commander.cpp └── robot_firmware_arduino └── robot_onboard └── robot_onboard.ino /README.md: -------------------------------------------------------------------------------- 1 | # robot_control_system 2 | Sample ros2 package for controlling differential robot with ros2 control 3 | 4 | 5 | #### To build whole package 6 | 7 | ``` colcon build --symlink-install ``` 8 | 9 | 10 | #### To run the controller package 11 | 12 | ``` ros2 launch robot_control_system start_actuator_demo.launch.py ``` 13 | 14 | 15 | #### To run the test package 16 | 17 | ``` ros2 run robot_control_system_tester start ``` 18 | 19 | 20 | #### Infra Required 21 | 22 | ![Infra](https://github.com/santoshbalaji/robot_control_system/blob/main/docs/infra.png?raw=true) 23 | 24 | 25 | #### Working video 26 | 27 | [![Part 1](https://img.youtube.com/vi/V1fUHFnJrdM/0.jpg)](https://youtu.be/V1fUHFnJrdM) 28 | 29 | [![Part 2](https://img.youtube.com/vi/gqeKTUTB10U/0.jpg)](https://youtu.be/gqeKTUTB10U) 30 | 31 | [![Part 3](https://img.youtube.com/vi/bVqyaFLVOis/0.jpg)](https://www.youtube.com/shorts/bVqyaFLVOis) 32 | -------------------------------------------------------------------------------- /docs/infra.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/santoshbalaji/robot_control_system/9306dd3b8b2c99119c3ae9f07e6de9e557369eb3/docs/infra.png -------------------------------------------------------------------------------- /robot_control_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_control_system) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | if(NOT CMAKE_CXX_STANDARD) 9 | set(CMAKE_CXX_STANDARD 17) 10 | endif() 11 | 12 | find_package(ament_cmake REQUIRED) 13 | find_package(hardware_interface REQUIRED) 14 | find_package(pluginlib REQUIRED) 15 | find_package(rclcpp REQUIRED) 16 | find_package(rclcpp_lifecycle REQUIRED) 17 | 18 | add_library( 19 | ${PROJECT_NAME} 20 | SHARED 21 | src/arduino_actuator_interface.cpp 22 | src/serial_port.cpp 23 | ) 24 | 25 | target_include_directories( 26 | ${PROJECT_NAME} 27 | PRIVATE 28 | include 29 | ) 30 | 31 | ament_target_dependencies( 32 | ${PROJECT_NAME} 33 | hardware_interface 34 | pluginlib 35 | rclcpp 36 | rclcpp_lifecycle 37 | ) 38 | 39 | target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") 40 | 41 | target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 42 | pluginlib_export_plugin_description_file(hardware_interface robot_control_system.xml) 43 | 44 | install( 45 | TARGETS ${PROJECT_NAME} 46 | DESTINATION lib 47 | ) 48 | 49 | install( 50 | DIRECTORY include/ 51 | DESTINATION include 52 | ) 53 | 54 | install( 55 | DIRECTORY config launch urdf rviz 56 | DESTINATION share/${PROJECT_NAME} 57 | ) 58 | 59 | if(BUILD_TESTING) 60 | find_package(ament_lint_auto REQUIRED) 61 | ament_lint_auto_find_test_dependencies() 62 | endif() 63 | 64 | ament_export_include_directories( 65 | include 66 | ) 67 | 68 | ament_export_libraries( 69 | ${PROJECT_NAME} 70 | ) 71 | 72 | ament_export_dependencies( 73 | hardware_interface 74 | pluginlib 75 | rclcpp 76 | rclcpp_lifecycle 77 | ) 78 | 79 | ament_package() 80 | -------------------------------------------------------------------------------- /robot_control_system/config/arduino_actuator_control.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 10 # Value in hertz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | position_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | position_trajectory_controller: 12 | ros__parameters: 13 | joints: 14 | - joint2 15 | 16 | command_interfaces: 17 | - velocity 18 | 19 | state_interfaces: 20 | - position 21 | - velocity 22 | 23 | state_publish_rate: 200.0 24 | action_monitor_rate: 20.0 25 | 26 | allow_partial_joints_goal: true 27 | open_loop_control: false 28 | allow_integration_in_goal_trajectories: true 29 | constraints: 30 | stopped_velocity_tolerance: 0.05 # Defaults to 0.01 31 | goal_time: 0.0 # Defaults to 0.0 (start immediately) 32 | 33 | gains: 34 | joint2: 35 | p: 2.0 36 | i: 1.0 37 | d: 1.0 38 | i_clamp: 1.0 39 | ff_velocity_scale: 2.0 40 | 41 | -------------------------------------------------------------------------------- /robot_control_system/include/robot_control_system/arduino_actuator_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_CONTROL_SYSTEM__ARDUINO_ACTUATOR_INTERFACE_HPP_ 2 | #define ROBOT_CONTROL_SYSTEM__ARDUINO_ACTUATOR_INTERFACE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "hardware_interface/handle.hpp" 9 | #include "hardware_interface/hardware_info.hpp" 10 | #include "hardware_interface/system_interface.hpp" 11 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 12 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 13 | 14 | #include "rclcpp/rclcpp.hpp" 15 | #include "rclcpp/macros.hpp" 16 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 17 | #include "rclcpp_lifecycle/state.hpp" 18 | 19 | #include "robot_control_system/serial_port.hpp" 20 | #include "robot_control_system/visibility_control.h" 21 | 22 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 23 | 24 | namespace robot_control_system 25 | { 26 | 27 | class ArduinoActuatorInterface : public hardware_interface::SystemInterface 28 | { 29 | public: 30 | RCLCPP_SHARED_PTR_DEFINITIONS(ArduinoActuatorInterface) 31 | 32 | ROBOT_CONTROL_SYSTEM_PUBLIC 33 | CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; 34 | 35 | ROBOT_CONTROL_SYSTEM_PUBLIC 36 | CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; 37 | 38 | ROBOT_CONTROL_SYSTEM_PUBLIC 39 | std::vector export_state_interfaces() override; 40 | 41 | ROBOT_CONTROL_SYSTEM_PUBLIC 42 | std::vector export_command_interfaces() override; 43 | 44 | ROBOT_CONTROL_SYSTEM_PUBLIC 45 | CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; 46 | 47 | ROBOT_CONTROL_SYSTEM_PUBLIC 48 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; 49 | 50 | ROBOT_CONTROL_SYSTEM_PUBLIC 51 | hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; 52 | 53 | ROBOT_CONTROL_SYSTEM_PUBLIC 54 | hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; 55 | 56 | private: 57 | double hw_start_sec_; 58 | double hw_stop_sec_; 59 | double hw_slowdown_; 60 | 61 | std::vector hw_commands_; 62 | std::vector hw_states_; 63 | std::vector encoder_readings_; 64 | 65 | SerialPort * serial_port; 66 | }; 67 | 68 | } // namespace robot_control_system 69 | 70 | #endif // ROBOT_CONTROL_SYSTEM__ARDUINO_ACTUATOR_INTERFACE_HPP_ 71 | -------------------------------------------------------------------------------- /robot_control_system/include/robot_control_system/serial_port.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_CONTROL_SYSTEM__SERIAL_PORT_HPP_ 2 | #define ROBOT_CONTROL_SYSTEM__SERIAL_PORT_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using namespace std; 13 | 14 | namespace robot_control_system 15 | { 16 | 17 | class SerialPort 18 | { 19 | public: 20 | SerialPort(string port); 21 | int connect(); 22 | int read_from_serial(int bytes_to_read, char * output); 23 | int write_to_serial(const char * data); 24 | void disconenct(); 25 | 26 | private: 27 | string port_; 28 | int file_descriptor_; 29 | }; 30 | 31 | } // namespace robot_control_system 32 | 33 | #endif // ROBOT_CONTROL_SYSTEM__SERIAL_PORT_HPP_ -------------------------------------------------------------------------------- /robot_control_system/include/robot_control_system/visibility_control.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_CONTROL_SYSTEM__VISIBILITY_CONTROL_H_ 2 | #define ROBOT_CONTROL_SYSTEM__VISIBILITY_CONTROL_H_ 3 | 4 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 5 | // https://gcc.gnu.org/wiki/Visibility 6 | 7 | #if defined _WIN32 || defined __CYGWIN__ 8 | #ifdef __GNUC__ 9 | #define ROBOT_CONTROL_SYSTEM_EXPORT __attribute__((dllexport)) 10 | #define ROBOT_CONTROL_SYSTEM_IMPORT __attribute__((dllimport)) 11 | #else 12 | #define ROBOT_CONTROL_SYSTEM_EXPORT __declspec(dllexport) 13 | #define ROBOT_CONTROL_SYSTEM_IMPORT __declspec(dllimport) 14 | #endif 15 | #ifdef ROBOT_CONTROL_SYSTEM_BUILDING_DLL 16 | #define ROBOT_CONTROL_SYSTEM_PUBLIC ROBOT_CONTROL_SYSTEM_EXPORT 17 | #else 18 | #define ROBOT_CONTROL_SYSTEM_PUBLIC ROBOT_CONTROL_SYSTEM_IMPORT 19 | #endif 20 | #define ROBOT_CONTROL_SYSTEM_PUBLIC_TYPE ROBOT_CONTROL_SYSTEM_PUBLIC 21 | #define ROBOT_CONTROL_SYSTEM_LOCAL 22 | #else 23 | #define ROBOT_CONTROL_SYSTEM_EXPORT __attribute__((visibility("default"))) 24 | #define ROBOT_CONTROL_SYSTEM_IMPORT 25 | #if __GNUC__ >= 4 26 | #define ROBOT_CONTROL_SYSTEM_PUBLIC __attribute__((visibility("default"))) 27 | #define ROBOT_CONTROL_SYSTEM_LOCAL __attribute__((visibility("hidden"))) 28 | #else 29 | #define ROBOT_CONTROL_SYSTEM_PUBLIC 30 | #define ROBOT_CONTROL_SYSTEM_LOCAL 31 | #endif 32 | #define ROBOT_CONTROL_SYSTEM_PUBLIC_TYPE 33 | #endif 34 | 35 | #endif // ROBOT_CONTROL_SYSTEM__VISIBILITY_CONTROL_H_ 36 | -------------------------------------------------------------------------------- /robot_control_system/launch/start_actuator_demo.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import RegisterEventHandler 3 | from launch.event_handlers import OnProcessExit 4 | from launch.substitutions import Command, FindExecutable, PathJoinSubstitution 5 | 6 | from launch_ros.actions import Node 7 | from launch_ros.substitutions import FindPackageShare 8 | 9 | 10 | def generate_launch_description(): 11 | # get urdf via xacro to show actuator movement 12 | robot_description_content = Command( 13 | [ 14 | PathJoinSubstitution([FindExecutable(name="xacro")]), 15 | " ", 16 | PathJoinSubstitution( 17 | [ 18 | FindPackageShare("robot_control_system"), 19 | "urdf", 20 | "arduino_actuator_control.xacro", 21 | ] 22 | ), 23 | ] 24 | ) 25 | robot_description = {"robot_description": robot_description_content} 26 | 27 | # configuration specifying type of controllers for joints and their configurable parameters 28 | robot_controllers = PathJoinSubstitution( 29 | [ 30 | FindPackageShare("robot_control_system"), 31 | "config", 32 | "arduino_actuator_control.yaml", 33 | ] 34 | ) 35 | 36 | # configuration for rviz 37 | rviz_config_file = PathJoinSubstitution( 38 | [FindPackageShare("robot_control_system"), "rviz", "control.rviz"] 39 | ) 40 | 41 | # node for controller manager which manages lifecycle of controllers defined for joints 42 | control_node = Node( 43 | package="controller_manager", 44 | executable="ros2_control_node", 45 | parameters=[robot_description, robot_controllers], 46 | output={ 47 | "stdout": "screen", 48 | "stderr": "screen", 49 | }, 50 | ) 51 | 52 | # node which publishes robot state including the robot description 53 | robot_state_pub_node = Node( 54 | package="robot_state_publisher", 55 | executable="robot_state_publisher", 56 | output="both", 57 | parameters=[robot_description], 58 | ) 59 | 60 | # node for launching rviz 61 | rviz_node = Node( 62 | package="rviz2", 63 | executable="rviz2", 64 | name="rviz2", 65 | output="log", 66 | arguments=["-d", rviz_config_file], 67 | ) 68 | 69 | # node to broadcasting joint states of various joints listed in urdf and configuration 70 | joint_state_broadcaster_spawner = Node( 71 | package="controller_manager", 72 | executable="spawner", 73 | arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], 74 | ) 75 | 76 | # node to start controllers associated with the joints 77 | robot_controller_spawner = Node( 78 | package="controller_manager", 79 | executable="spawner", 80 | arguments=["position_trajectory_controller", "-c", "/controller_manager"], 81 | ) 82 | 83 | # delay rviz start after `joint_state_broadcaster` 84 | delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( 85 | event_handler=OnProcessExit( 86 | target_action=joint_state_broadcaster_spawner, 87 | on_exit=[rviz_node], 88 | ) 89 | ) 90 | 91 | # delay start of robot_controller after `joint_state_broadcaster` 92 | delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( 93 | event_handler=OnProcessExit( 94 | target_action=joint_state_broadcaster_spawner, 95 | on_exit=[robot_controller_spawner], 96 | ) 97 | ) 98 | 99 | nodes = [ 100 | control_node, 101 | robot_state_pub_node, 102 | joint_state_broadcaster_spawner, 103 | delay_rviz_after_joint_state_broadcaster_spawner, 104 | delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, 105 | ] 106 | 107 | return LaunchDescription(nodes) 108 | -------------------------------------------------------------------------------- /robot_control_system/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_control_system 5 | 0.0.0 6 | Sample package for ROS2 control system basics 7 | santosh balaji 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | hardware_interface 13 | pluginlib 14 | rclcpp 15 | rclcpp_lifecycle 16 | 17 | ament_lint_auto 18 | ament_lint_common 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /robot_control_system/robot_control_system.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | Test description 6 | 7 | -------------------------------------------------------------------------------- /robot_control_system/rviz/control.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 87 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 778 9 | - Class: rviz_common/Selection 10 | Name: Selection 11 | - Class: rviz_common/Tool Properties 12 | Expanded: 13 | - /2D Goal Pose1 14 | - /Publish Point1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz_common/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | Visualization Manager: 23 | Class: "" 24 | Displays: 25 | - Alpha: 0.5 26 | Cell Size: 1 27 | Class: rviz_default_plugins/Grid 28 | Color: 160; 160; 164 29 | Enabled: true 30 | Line Style: 31 | Line Width: 0.029999999329447746 32 | Value: Lines 33 | Name: Grid 34 | Normal Cell Count: 0 35 | Offset: 36 | X: 0 37 | Y: 0 38 | Z: 0 39 | Plane: XY 40 | Plane Cell Count: 10 41 | Reference Frame: 42 | Value: true 43 | - Alpha: 1 44 | Class: rviz_default_plugins/RobotModel 45 | Collision Enabled: false 46 | Description File: "" 47 | Description Source: Topic 48 | Description Topic: 49 | Depth: 5 50 | Durability Policy: Volatile 51 | History Policy: Keep Last 52 | Reliability Policy: Reliable 53 | Value: /robot_description 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | rotor: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | world: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Name: RobotModel 76 | TF Prefix: "" 77 | Update Interval: 0 78 | Value: true 79 | Visual Enabled: true 80 | - Class: rviz_default_plugins/TF 81 | Enabled: true 82 | Frame Timeout: 15 83 | Frames: 84 | All Enabled: true 85 | base: 86 | Value: true 87 | rotor: 88 | Value: true 89 | world: 90 | Value: true 91 | Marker Scale: 1 92 | Name: TF 93 | Show Arrows: true 94 | Show Axes: true 95 | Show Names: false 96 | Tree: 97 | world: 98 | base: 99 | rotor: 100 | {} 101 | Update Interval: 0 102 | Value: true 103 | Enabled: true 104 | Global Options: 105 | Background Color: 48; 48; 48 106 | Fixed Frame: base 107 | Frame Rate: 30 108 | Name: root 109 | Tools: 110 | - Class: rviz_default_plugins/Interact 111 | Hide Inactive Objects: true 112 | - Class: rviz_default_plugins/MoveCamera 113 | - Class: rviz_default_plugins/Select 114 | - Class: rviz_default_plugins/FocusCamera 115 | - Class: rviz_default_plugins/Measure 116 | Line color: 128; 128; 0 117 | - Class: rviz_default_plugins/SetInitialPose 118 | Covariance x: 0.25 119 | Covariance y: 0.25 120 | Covariance yaw: 0.06853891909122467 121 | Topic: 122 | Depth: 5 123 | Durability Policy: Volatile 124 | History Policy: Keep Last 125 | Reliability Policy: Reliable 126 | Value: /initialpose 127 | - Class: rviz_default_plugins/SetGoal 128 | Topic: 129 | Depth: 5 130 | Durability Policy: Volatile 131 | History Policy: Keep Last 132 | Reliability Policy: Reliable 133 | Value: /goal_pose 134 | - Class: rviz_default_plugins/PublishPoint 135 | Single click: true 136 | Topic: 137 | Depth: 5 138 | Durability Policy: Volatile 139 | History Policy: Keep Last 140 | Reliability Policy: Reliable 141 | Value: /clicked_point 142 | Transformation: 143 | Current: 144 | Class: rviz_default_plugins/TF 145 | Value: true 146 | Views: 147 | Current: 148 | Class: rviz_default_plugins/XYOrbit 149 | Distance: 0.9189692735671997 150 | Enable Stereo Rendering: 151 | Stereo Eye Separation: 0.05999999865889549 152 | Stereo Focal Distance: 1 153 | Swap Stereo Eyes: false 154 | Value: false 155 | Focal Point: 156 | X: 0.31069105863571167 157 | Y: -0.15606188774108887 158 | Z: 4.887580871582031e-06 159 | Focal Shape Fixed Size: true 160 | Focal Shape Size: 0.05000000074505806 161 | Invert Z Axis: false 162 | Name: Current View 163 | Near Clip Distance: 0.009999999776482582 164 | Pitch: 1.0403987169265747 165 | Target Frame: 166 | Value: XYOrbit (rviz_default_plugins) 167 | Yaw: 4.748561382293701 168 | Saved: ~ 169 | Window Geometry: 170 | Displays: 171 | collapsed: false 172 | Height: 1016 173 | Hide Left Dock: false 174 | Hide Right Dock: false 175 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001100000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b40000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 176 | Selection: 177 | collapsed: false 178 | Tool Properties: 179 | collapsed: false 180 | Views: 181 | collapsed: false 182 | Width: 1850 183 | X: 70 184 | Y: 27 185 | -------------------------------------------------------------------------------- /robot_control_system/src/arduino_actuator_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace robot_control_system 4 | { 5 | 6 | // Different states in interfaces 7 | // Unconfigured, Inactive, Active, Finalized 8 | 9 | 10 | /** 11 | * Function which gets invoked at starting of lifecycle in controllers 12 | */ 13 | CallbackReturn ArduinoActuatorInterface::on_init( 14 | const hardware_interface::HardwareInfo & info) 15 | { 16 | try 17 | { 18 | if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) 19 | { 20 | return CallbackReturn::ERROR; 21 | } 22 | 23 | hw_start_sec_ = stod(info_.hardware_parameters["hw_start_duration_sec"]); 24 | hw_stop_sec_ = stod(info_.hardware_parameters["hw_stop_duration_sec"]); 25 | hw_slowdown_ = stod(info_.hardware_parameters["hw_slowdown"]); 26 | 27 | int total_states = 0, total_commands = 0; 28 | 29 | // get total required states and commands 30 | for (uint i = 0; i < info_.joints.size(); i++) 31 | { 32 | for (uint j = 0; j < info_.joints[i].state_interfaces.size(); j++) 33 | { 34 | total_states = total_states + 1; 35 | } 36 | } 37 | 38 | for (uint i = 0; i < info_.joints.size(); i++) 39 | { 40 | for (uint j = 0; j < info_.joints[i].command_interfaces.size(); j++) 41 | { 42 | total_commands = total_commands + 1; 43 | } 44 | } 45 | 46 | hw_states_.resize(total_states, std::numeric_limits::quiet_NaN()); 47 | hw_commands_.resize(total_commands, std::numeric_limits::quiet_NaN()); 48 | encoder_readings_.resize(total_commands, std::numeric_limits::quiet_NaN()); 49 | 50 | for (const hardware_interface::ComponentInfo & joint : info_.joints) 51 | { 52 | // checking if required command interfaces is one 53 | if (joint.command_interfaces.size() != 1) 54 | { 55 | RCLCPP_FATAL( 56 | rclcpp::get_logger("arduino_actuator_interface"), 57 | "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), 58 | joint.command_interfaces.size()); 59 | return CallbackReturn::ERROR; 60 | } 61 | 62 | // checking if required command interfaces is related to velocity 63 | if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) 64 | { 65 | RCLCPP_FATAL( 66 | rclcpp::get_logger("arduino_actuator_interface"), 67 | "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), 68 | joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); 69 | return CallbackReturn::ERROR; 70 | } 71 | 72 | // checking if required states interfaces is two 73 | if (joint.state_interfaces.size() != 2) 74 | { 75 | RCLCPP_FATAL( 76 | rclcpp::get_logger("arduino_actuator_interface"), 77 | "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), 78 | joint.state_interfaces.size()); 79 | return CallbackReturn::ERROR; 80 | } 81 | 82 | // checking if required state interfaces is related to position 83 | if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) 84 | { 85 | RCLCPP_FATAL( 86 | rclcpp::get_logger("arduino_actuator_interface"), 87 | "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), 88 | joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); 89 | return CallbackReturn::ERROR; 90 | } 91 | 92 | // checking if required state interfaces is related to velocity 93 | if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) 94 | { 95 | RCLCPP_FATAL( 96 | rclcpp::get_logger("arduino_actuator_interface"), 97 | "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), 98 | joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); 99 | return CallbackReturn::ERROR; 100 | } 101 | } 102 | } 103 | catch(std::exception &e) 104 | { 105 | RCLCPP_FATAL( 106 | rclcpp::get_logger("arduino_actuator_interface"), 107 | "Error: %s", e.what() 108 | ); 109 | return CallbackReturn::ERROR; 110 | } 111 | return CallbackReturn::SUCCESS; 112 | } 113 | 114 | /** 115 | * Function to send reference of state variables to parent controller 116 | * These values can be read or modified by parent controller 117 | * Executed after on_init function 118 | */ 119 | std::vector ArduinoActuatorInterface::export_state_interfaces() 120 | { 121 | RCLCPP_INFO( 122 | rclcpp::get_logger("arduino_actuator_interface"), "exporting state interfaces"); 123 | std::vector state_interfaces; 124 | 125 | try 126 | { 127 | int current_state = 0; 128 | for (uint i = 0; i < info_.joints.size(); i++) 129 | { 130 | for (uint j = 0; j < info_.joints[i].state_interfaces.size(); j++) 131 | { 132 | // reference of states are added here for allowing of read and modification by parent controller 133 | state_interfaces.emplace_back(hardware_interface::StateInterface( 134 | info_.joints[i].name, info_.joints[i].state_interfaces[j].name, &hw_states_[current_state])); 135 | current_state = current_state + 1; 136 | } 137 | } 138 | } 139 | catch(std::exception &e) 140 | { 141 | RCLCPP_FATAL( 142 | rclcpp::get_logger("arduino_actuator_interface"), 143 | "Error: %s", e.what() 144 | ); 145 | } 146 | 147 | return state_interfaces; 148 | } 149 | 150 | /** 151 | * Function to send reference of command variables to parent controller 152 | * These values can be read or modified by parent controller 153 | * Executed after on_init function 154 | */ 155 | std::vector ArduinoActuatorInterface::export_command_interfaces() 156 | { 157 | RCLCPP_INFO( 158 | rclcpp::get_logger("arduino_actuator_interface"), "exporting command interfaces"); 159 | std::vector command_interfaces; 160 | 161 | try 162 | { 163 | int current_command = 0; 164 | for (uint i = 0; i < info_.joints.size(); i++) 165 | { 166 | for (uint j = 0; j < info_.joints[i].command_interfaces.size(); j++) 167 | { 168 | // reference of commands are added here for allowing of read and modification by parent controller 169 | command_interfaces.emplace_back(hardware_interface::CommandInterface( 170 | info_.joints[i].name, info_.joints[i].command_interfaces[j].name, &hw_commands_[current_command])); 171 | current_command = current_command + 1; 172 | } 173 | } 174 | } 175 | catch(std::exception &e) 176 | { 177 | RCLCPP_FATAL( 178 | rclcpp::get_logger("arduino_actuator_interface"), 179 | "Error: %s", e.what() 180 | ); 181 | } 182 | return command_interfaces; 183 | } 184 | 185 | /** 186 | * Function which gets executed after export_state_interfaces and export_command_interfaces function 187 | * Intialize all state and command based variables here 188 | */ 189 | CallbackReturn ArduinoActuatorInterface::on_configure( 190 | const rclcpp_lifecycle::State & previous_state) 191 | { 192 | RCLCPP_INFO( 193 | rclcpp::get_logger("arduino_actuator_interface"), "configuring hardware"); 194 | try 195 | { 196 | for (int i = 0; i < hw_start_sec_; i++) 197 | { 198 | rclcpp::sleep_for(std::chrono::seconds(1)); 199 | RCLCPP_INFO( 200 | rclcpp::get_logger("arduino_actuator_interface"), "%.1f seconds left in configuring", 201 | hw_start_sec_ - i); 202 | } 203 | 204 | // states are initialized 205 | for (uint i = 0; i < hw_states_.size(); i++) 206 | { 207 | hw_states_[i] = 0; 208 | } 209 | 210 | // commands are initialized 211 | for (uint i = 0; i < hw_commands_.size(); i++) 212 | { 213 | hw_commands_[i] = 0; 214 | encoder_readings_[i] = 0; 215 | } 216 | 217 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "successfully configured"); 218 | } 219 | catch(std::exception &e) 220 | { 221 | RCLCPP_FATAL( 222 | rclcpp::get_logger("arduino_actuator_interface"), 223 | "Error: %s", e.what() 224 | ); 225 | return CallbackReturn::ERROR; 226 | } 227 | return CallbackReturn::SUCCESS; 228 | } 229 | 230 | /** 231 | * Function which gets executed after on_configure function 232 | * This function gets invoked on activating the controller 233 | */ 234 | CallbackReturn ArduinoActuatorInterface::on_activate( 235 | const rclcpp_lifecycle::State & previous_state) 236 | { 237 | RCLCPP_INFO( 238 | rclcpp::get_logger("arduino_actuator_interface"), "activating now"); 239 | 240 | try 241 | { 242 | for (int i = 0; i < hw_start_sec_; i++) 243 | { 244 | rclcpp::sleep_for(std::chrono::seconds(1)); 245 | RCLCPP_INFO( 246 | rclcpp::get_logger("arduino_actuator_interface"), "%.1f seconds left in activating", 247 | hw_start_sec_ - i); 248 | } 249 | 250 | // initialising and connecting to serial communication module 251 | this->serial_port = new SerialPort("/dev/ttyACM0"); 252 | this->serial_port->connect(); 253 | } 254 | catch(std::exception &e) 255 | { 256 | RCLCPP_FATAL( 257 | rclcpp::get_logger("arduino_actuator_interface"), 258 | "Error: %s", e.what() 259 | ); 260 | return CallbackReturn::ERROR; 261 | } 262 | 263 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "successfully activated"); 264 | return CallbackReturn::SUCCESS; 265 | } 266 | 267 | /** 268 | * This function gets invoked on de-activating the controller 269 | */ 270 | CallbackReturn ArduinoActuatorInterface::on_deactivate( 271 | const rclcpp_lifecycle::State & previous_state) 272 | { 273 | RCLCPP_INFO( 274 | rclcpp::get_logger("arduino_actuator_interface"), "deactivating now"); 275 | 276 | try 277 | { 278 | for (int i = 0; i < hw_stop_sec_; i++) 279 | { 280 | rclcpp::sleep_for(std::chrono::seconds(1)); 281 | RCLCPP_INFO( 282 | rclcpp::get_logger("arduino_actuator_interface"), "%.1f seconds left in deactivating", 283 | hw_stop_sec_ - i); 284 | } 285 | 286 | // disconnecting from serial communication module 287 | this->serial_port->disconenct(); 288 | } 289 | catch(std::exception &e) 290 | { 291 | RCLCPP_FATAL( 292 | rclcpp::get_logger("arduino_actuator_interface"), 293 | "Error: %s", e.what() 294 | ); 295 | return CallbackReturn::ERROR; 296 | } 297 | 298 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "successfully deactivated"); 299 | return CallbackReturn::SUCCESS; 300 | } 301 | 302 | /** 303 | * Function which gets invoked at every configured update frequency 304 | * Logic of converting sensor values to states happens here 305 | */ 306 | hardware_interface::return_type ArduinoActuatorInterface::read(const rclcpp::Time & time, const rclcpp::Duration & period) 307 | { 308 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "Reading"); 309 | 310 | try 311 | { 312 | double radius = 0.038; 313 | double full_rotation_encoder_count = 873; 314 | 315 | char test_data[256]; 316 | this->serial_port->read_from_serial(256, test_data); 317 | double current_encoder_reading = std::stod(test_data); 318 | 319 | double total_elapsed_count = current_encoder_reading - this->encoder_readings_[0]; 320 | // calculating rate of change by getting the circumference of wheel and multiplying it with time interval 321 | // 2 * pi * radius of the wheel * (elapsed encoder count / total count needed by encoder for full rotation) * elapsed time period in hz 322 | double velocity = 2 * 3.14 * radius * (total_elapsed_count / full_rotation_encoder_count) * 10; //(frequency is 10 hz) 323 | 324 | this->encoder_readings_[0] = current_encoder_reading; 325 | 326 | // computing angular displacement (linear velocity / (radius * elasped time in hz)) 327 | double radians = hw_states_[0] + (velocity / (radius * 10)); 328 | hw_states_[0] = radians; 329 | hw_states_[1] = velocity; 330 | 331 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "%f %f %f %f", current_encoder_reading, total_elapsed_count, hw_states_[0], hw_states_[1]); 332 | } 333 | catch(std::exception &e) 334 | { 335 | RCLCPP_FATAL( 336 | rclcpp::get_logger("arduino_actuator_interface"), 337 | "Error: %s", e.what() 338 | ); 339 | return hardware_interface::return_type::ERROR; 340 | } 341 | 342 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "Joints successfully read"); 343 | return hardware_interface::return_type::OK; 344 | } 345 | 346 | /** 347 | * Function which gets invoked at every configured update frequency 348 | * Based on commands obtained external actuators or other devices can be controlled from here 349 | */ 350 | hardware_interface::return_type ArduinoActuatorInterface::write(const rclcpp::Time & time, const rclcpp::Duration & period) 351 | { 352 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "Writing"); 353 | 354 | try 355 | { 356 | // normalising threshold values (normalized_value = ((value - old_min) / (old_max - old_min)) * (new_max - new_min) + new_min) 357 | // 101, 0 - max, min command value to be obtained from control loop 358 | // 99, 15 - max, min command value to be sent for Arduino PWM control 359 | double threshold = ((hw_commands_[0]) / (101)) * (99 - 15) + 15; 360 | // Format for serial data (pwm_signal_value;forward;reverse) 361 | string data = std::to_string((int) threshold) + ";0;1;\n"; 362 | this->serial_port->write_to_serial(data.c_str()); 363 | RCLCPP_INFO(rclcpp::get_logger("arduino_actuator_interface"), "Writing %s", data.c_str()); 364 | } 365 | catch(std::exception &e) 366 | { 367 | RCLCPP_FATAL( 368 | rclcpp::get_logger("arduino_actuator_interface"), 369 | "Error: %s", e.what() 370 | ); 371 | return hardware_interface::return_type::ERROR; 372 | } 373 | 374 | RCLCPP_INFO( 375 | rclcpp::get_logger("arduino_actuator_interface"), "Joints successfully written %f", hw_commands_[0]); 376 | 377 | return hardware_interface::return_type::OK; 378 | } 379 | 380 | } // namespace robot_control_system 381 | 382 | #include "pluginlib/class_list_macros.hpp" 383 | 384 | PLUGINLIB_EXPORT_CLASS( 385 | robot_control_system::ArduinoActuatorInterface, hardware_interface::SystemInterface) 386 | -------------------------------------------------------------------------------- /robot_control_system/src/serial_port.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using SerialPort = robot_control_system::SerialPort; 4 | 5 | SerialPort::SerialPort(string port) 6 | { 7 | this->port_ = port; 8 | } 9 | 10 | /** 11 | * Function for connecting with serial controller 12 | */ 13 | int SerialPort::connect() 14 | { 15 | this->file_descriptor_ = open(this->port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); 16 | if (this->file_descriptor_ == -1) 17 | { 18 | std::cout << "Error: Unable to open serial port." << std::endl; 19 | return -1; 20 | } 21 | 22 | struct termios options; 23 | tcgetattr(this->file_descriptor_, &options); 24 | options.c_cflag = B9600 | CS8 | CLOCAL | CREAD; 25 | options.c_iflag = IGNPAR; 26 | options.c_oflag = 0; 27 | options.c_lflag = 0; 28 | tcflush(this->file_descriptor_, TCIFLUSH); 29 | tcsetattr(this->file_descriptor_, TCSANOW, &options); 30 | 31 | return 0; 32 | } 33 | 34 | /** 35 | * Function to read from serial controllers 36 | */ 37 | int SerialPort::read_from_serial(int bytes_to_read, char * output) 38 | { 39 | int bytes_read = read(this->file_descriptor_, output, bytes_to_read); 40 | if (bytes_read < 0) 41 | { 42 | std::cout << "Error: Unable to read from serial port." << std::endl; 43 | return -1; 44 | } 45 | return 0; 46 | } 47 | 48 | /** 49 | * Function to write on serial controllers 50 | */ 51 | int SerialPort::write_to_serial(const char * data) 52 | { 53 | int bytes_written = write(this->file_descriptor_, data, sizeof(data)); 54 | if (bytes_written < 0) 55 | { 56 | std::cout << "Error: Unable to write to serial port." << std::endl; 57 | return -1; 58 | } 59 | return 0; 60 | } 61 | 62 | /** 63 | * Function to disconnect from serial controller 64 | */ 65 | void SerialPort::disconenct() 66 | { 67 | if(this->file_descriptor_ != -1) 68 | { 69 | close(this->file_descriptor_); 70 | } 71 | } -------------------------------------------------------------------------------- /robot_control_system/urdf/arduino_actuator_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | robot_control_system/ArduinoActuatorInterface 69 | 0 70 | 3.0 71 | 100 72 | 73 | 74 | 75 | 76 | -1 77 | 1 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /robot_control_system_tester/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_control_system_tester) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(trajectory_msgs REQUIRED) 15 | 16 | include_directories(include) 17 | 18 | add_executable(start src/robot_control_commander.cpp) 19 | ament_target_dependencies(start rclcpp trajectory_msgs) 20 | 21 | install( 22 | DIRECTORY include/ 23 | DESTINATION include/ 24 | ) 25 | 26 | install(TARGETS 27 | start 28 | DESTINATION lib/${PROJECT_NAME}) 29 | 30 | if(BUILD_TESTING) 31 | find_package(ament_lint_auto REQUIRED) 32 | ament_lint_auto_find_test_dependencies() 33 | endif() 34 | 35 | ament_package() 36 | -------------------------------------------------------------------------------- /robot_control_system_tester/include/robot_control_system_tester/robot_control_commander.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_CONTROL_SYSTEM_TESTER__ROBOT_CONTROL_COMMANDER_HPP_ 2 | #define ROBOT_CONTROL_SYSTEM_TESTER__ROBOT_CONTROL_COMMANDER_HPP_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | using std::chrono_literals::operator""ms; 12 | using std::chrono::duration_cast; 13 | using std::chrono::milliseconds; 14 | using std::chrono::system_clock; 15 | 16 | namespace robot_control_system_tester 17 | { 18 | class RobotControlCommander : public rclcpp::Node 19 | { 20 | public: 21 | RobotControlCommander(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); 22 | void timer_callback(); 23 | 24 | private: 25 | rclcpp::Publisher< 26 | trajectory_msgs::msg::JointTrajectory>::SharedPtr robot_control_publisher_; 27 | rclcpp::TimerBase::SharedPtr background_process_timer_; 28 | }; 29 | } // namespace robot_control_system_tester 30 | 31 | #endif // ROBOT_CONTROL_SYSTEM_TESTER__ROBOT_CONTROL_COMMANDER_HPP_ -------------------------------------------------------------------------------- /robot_control_system_tester/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_control_system_tester 5 | 0.0.0 6 | TODO: Package description 7 | santoshartc 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /robot_control_system_tester/src/robot_control_commander.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace robot_control_system_tester 4 | { 5 | RobotControlCommander::RobotControlCommander( 6 | const rclcpp::NodeOptions & options) : Node("robot_control_commander", options) 7 | { 8 | // topic to which joint trajectories to be passed 9 | this->robot_control_publisher_ = this->create_publisher( 10 | "/position_trajectory_controller/joint_trajectory", 10); 11 | 12 | this->background_process_timer_ = this->create_wall_timer( 13 | 4000ms, std::bind(&RobotControlCommander::timer_callback, this)); 14 | } 15 | 16 | void RobotControlCommander::timer_callback() 17 | { 18 | std::vector joint_names = {"joint2"}; 19 | trajectory_msgs::msg::JointTrajectory joint_trajectory; 20 | joint_trajectory.joint_names = joint_names; 21 | std::vector joint_trajectory_points; 22 | trajectory_msgs::msg::JointTrajectoryPoint joint_trajectory_point; 23 | joint_trajectory_point.positions = {50}; // position is set for joint 2 24 | joint_trajectory_point.accelerations = {}; 25 | joint_trajectory_point.effort = {}; 26 | joint_trajectory_point.velocities = {0.2}; // velocity is set for joint 2 27 | joint_trajectory_points.push_back(joint_trajectory_point); 28 | joint_trajectory.points = joint_trajectory_points; 29 | 30 | this->robot_control_publisher_->publish(joint_trajectory); 31 | } 32 | } 33 | 34 | int main(int argc, char * argv[]) 35 | { 36 | rclcpp::init(argc, argv); 37 | rclcpp::spin(std::make_shared()); 38 | rclcpp::shutdown(); 39 | return 0; 40 | } -------------------------------------------------------------------------------- /robot_firmware_arduino/robot_onboard/robot_onboard.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define ENCAF 2 4 | #define ENCAR A1 5 | #define PWMA 5 6 | #define MAF A5 7 | #define MAR A4 8 | 9 | Encoder encoder(ENCAF, ENCAR); 10 | 11 | int pwm_value = 0, forward = 0, reverse = 0; 12 | long encoder_reading = 0; 13 | String data; 14 | long elapsed_time = 0; 15 | 16 | void setup() 17 | { 18 | Serial.begin(9600); 19 | pinMode(PWMA, OUTPUT); 20 | pinMode(MAF, OUTPUT); 21 | pinMode(MAR, OUTPUT); 22 | } 23 | 24 | void loop() 25 | { 26 | // to run the code without delay command (run the logic every 100ms) 27 | if(millis() - elapsed_time >= 100) 28 | { 29 | analogWrite(PWMA, pwm_value); 30 | digitalWrite(MAF, forward); 31 | digitalWrite(MAR, reverse); 32 | 33 | while(Serial.available() > 0) 34 | { 35 | char s = Serial.read(); 36 | data += s; 37 | if(s == '\n') 38 | { 39 | int prev = 0, count = 0; 40 | for (int i = 0; i < data.length(); i++) 41 | { 42 | if(data[i] == ';') 43 | { 44 | if(count == 0) 45 | { 46 | pwm_value = data.substring(prev, i).toInt(); 47 | } 48 | else if(count == 1) 49 | { 50 | forward = data.substring(prev, i).toInt(); 51 | } 52 | else if(count == 2) 53 | { 54 | reverse = data.substring(prev, i).toInt(); 55 | } 56 | prev = i + 1; 57 | count = count + 1; 58 | } 59 | } 60 | data = ""; 61 | } 62 | } 63 | 64 | encoder_reading = encoder.read(); 65 | Serial.println(encoder_reading); 66 | 67 | elapsed_time = millis(); 68 | } 69 | } 70 | --------------------------------------------------------------------------------