Add RS232 code to hardware interface.

diff --git a/q2d_bringup/launch/hardware.launch.xml b/q2d_bringup/launch/hardware.launch.xml
new file mode 100644 (file)
index 0000000..8a47983
--- /dev/null
+++ b/q2d_bringup/launch/hardware.launch.xml
@@ -0,0 +1,38 @@
+<!--******************************************************************************
+                          Quanser 2DSFJE Bringup
+                            Hardware Launch File
+          Copyright (C) 2023 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        Geneal Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+        
+*******************************************************************************-->
+
+<launch>
+       <arg name="gui" default="true"/>
+
+       <arg name="controller" default="group_bypass"/>
+       <arg name="config" default="$(find-pkg-share q2d_bringup)/config/$(var controller).yaml"/>
+
+       <include file="$(find-pkg-share q2d_hardware)/launch/controller_manager.launch.xml"/>
+       
+       <include file="$(find-pkg-share q2d_bringup)/launch/$(var controller).launch.xml">
+               <arg name="config" value="$(var config)"/>
+               <arg name="use_sim_time" value="false"/>
+       </include>
+       
+       <include if="$(var gui)" file="$(find-pkg-share q2d_description)/launch/display.launch.xml">
+               <arg name="gui" value="false"/>
+       </include>
+</launch>

diff --git a/q2d_hardware/CMakeLists.txt b/q2d_hardware/CMakeLists.txt
index 8451117..9331a8a 100644 (file)
--- a/q2d_hardware/CMakeLists.txt
+++ b/q2d_hardware/CMakeLists.txt
@@ -24,6 +24,9 @@ ament_target_dependencies(
   "pluginlib"
 )
 
+target_include_directories(q2d_hardware PUBLIC $ENV{HOME}/include)
+target_link_libraries(q2d_hardware $ENV{HOME}/lib/libaic_lib.a)
+
 pluginlib_export_plugin_description_file(hardware_interface q2d_hardware.xml)
 
 # Causes the visibility macros to use dllexport rather than dllimport,

diff --git a/q2d_hardware/include/q2d_hardware/q2d_system.hpp b/q2d_hardware/include/q2d_hardware/q2d_system.hpp
index bf8a395..6003ed9 100644 (file)
--- a/q2d_hardware/include/q2d_hardware/q2d_system.hpp
+++ b/q2d_hardware/include/q2d_hardware/q2d_system.hpp
@@ -30,6 +30,8 @@
 
 #include "q2d_hardware/visibility_control.h"
 
+#include "aic.h"
+
 namespace q2d_hardware
 {
        class Q2dSystemHardware: public hardware_interface::SystemInterface
@@ -65,6 +67,13 @@ namespace q2d_hardware
                std::vector<double> command_;
                std::vector<double> position_;
                std::vector<double> velocity_;
+               std::vector<double> effort_;
+               
+               std::vector<std::unique_ptr<aic>> board_;
+               std::vector<double> Kt_;
+               std::vector<double> Ka_;
+               std::vector<double> Ra_;
+               std::vector<double> N_;
        };
 
 }  // namespace q2d_hardware

diff --git a/q2d_hardware/src/q2d_system.cpp b/q2d_hardware/src/q2d_system.cpp
index 5367953..fddb645 100644 (file)
--- a/q2d_hardware/src/q2d_system.cpp
+++ b/q2d_hardware/src/q2d_system.cpp
@@ -34,12 +34,26 @@ namespace q2d_hardware
                if(hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
                        return CallbackReturn::ERROR;
 
-               /* initialize hardware */
+               /* Initialize Hardware */
+               board_.resize(info_.joints.size());             
 
                position_.resize(info_.joints.size());
                velocity_.resize(info_.joints.size());
+               effort_.resize(info_.joints.size());
+               
                command_.resize(info_.joints.size());
 
+               Kt_.resize(info_.joints.size());
+               Kt_[0]=0.119;
+               Kt_[1]=0.0234;
+               Ka_.resize(info_.joints.size());
+               Ka_[0]=0.119;
+               Ka_[1]=0.0234;
+               Ra_.resize(info_.joints.size());
+               Ra_[0]=11.5;
+               Ra_[1]=2.32;
+               N_.resize(info_.joints.size());
+
                for(const hardware_interface::ComponentInfo & joint : info_.joints)
                {
                        if(joint.command_interfaces.size() != 1)
@@ -102,6 +116,16 @@ namespace q2d_hardware
        hardware_interface::CallbackReturn Q2dSystemHardware::on_configure(const rclcpp_lifecycle::State & /*previous_stte*/)
        {
                /* configure hardware */
+               aic_comm_config_t param;
+               param.aic_comm_device=rs232;
+               
+               for(auto i=0u;i < board_.size();i++)
+               {
+                       param.aic_serial_port="/dev/tty/USB"+ ('0'+i);
+                       if((board_[i]=std::make_unique<aic>(param)) == NULL)
+                               RCLCPP_ERROR_STREAM(rclcpp::get_logger("Q2dSystemHardware"),
+                                       "Can not configure " << param.aic_serial_port << " for interfacing with joint " << i);
+               }
                return CallbackReturn::SUCCESS;
        }
         
@@ -137,28 +161,44 @@ namespace q2d_hardware
                {
                        position_[i]=0;
                        velocity_[i]=0;
+                       effort_[i]=0;
                        command_[i]=0;
                }
                 
-               /* activate hardware */                
+               /* activate hardware */
+               for(auto i=0u;i < board_.size();i++) board_[i]->set_motor_voltage(0);
+                        
                return CallbackReturn::SUCCESS;
        }
 
        hardware_interface::CallbackReturn Q2dSystemHardware::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/)
        {
                /* deactivate hardware */
+               for(auto i=0u;i < board_.size();i++) board_[i]->~aic();
+
                return CallbackReturn::SUCCESS;;
        }
 
-       hardware_interface::return_type Q2dSystemHardware::read(const rclcpp::Time &/*time*/,const rclcpp::Duration &/*period*/)
+       hardware_interface::return_type Q2dSystemHardware::read(const rclcpp::Time &/*time*/,const rclcpp::Duration &period)
        {
                /* read hardware */
+               for(auto i=0u;i < board_.size();i++)
+               {
+                       auto dp=board_[i]->read_displacement_sensors();
+                       velocity_[i]=(dp.joint_displacement-position_[i])/period.seconds();
+                       position_[i]=dp.joint_displacement;
+                       effort_[i]=command_[i];
+               }
+               
                return hardware_interface::return_type::OK;
        }
 
        hardware_interface::return_type Q2dSystemHardware::write(const rclcpp::Time &/*time*/,const rclcpp::Duration &/*period*/)
        {
                /* write hardware */
+               for(auto i=0u;i < board_.size();i++)
+                       board_[i]->set_motor_voltage(N_[i]*Ra_[i]/Kt_[i]*command_[i]+Ka_[i]/N_[i]*velocity_[i]);
+
                return hardware_interface::return_type::OK;
        }