From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Tue, 13 Jun 2023 05:10:40 +0000 (-0300)
Subject: Add RS232 code to hardware interface.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=24bdb48cd49acc6016cf1184b7f9d2b1c567c201;p=q2d.git

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
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
--- 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
--- 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
--- 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;
 	}