diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index 7ec64e1e..d8b88c7d 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/asv_setup/config/robots/freya.yaml @@ -50,3 +50,10 @@ scaling: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] computer: "pc-debug" + + + thruster_interface: + publishing_rate: 1000 # [millisecond], Publishing 6 temperature values (ESC1, ESC2, ESC3, ESC4, AMBIENT1, AMBIENT2) + # and the status received from the arduino controlling the ESC + PWM_min: 1300 # Recommended : 1100 + PWM_max: 1700 # Recommended : 1900 diff --git a/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp index 890a53f5..0a8ae074 100644 --- a/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp +++ b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp @@ -37,5 +37,5 @@ uint8_t read_hardware_statusFromI2C(int file); //--------------------------INTERPOLATION-------------------------- void get_pwm_table(); -uint16_t interpolate(double force); -std::vector interpolate_all(std::vector &force_values); +uint16_t interpolate(double force, int PWM_min, int PWM_max); +std::vector interpolate_all(std::vector &force_values, int PWM_min = 1100, int PWM_max = 1900); diff --git a/motion/thruster_interface/launch/thruster_interface.launch.py b/motion/thruster_interface/launch/thruster_interface.launch.py index c8e88718..c73c36e8 100644 --- a/motion/thruster_interface/launch/thruster_interface.launch.py +++ b/motion/thruster_interface/launch/thruster_interface.launch.py @@ -4,13 +4,18 @@ from launch_ros.actions import Node def generate_launch_description(): + #Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("thruster_interface"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go where the yaml file is located at + 'freya.yaml' + ) thruster_interface_node = Node( package='thruster_interface', executable='thruster_interface_node', name='thruster_interface_node', - parameters=[], output='screen', + parameters=[yaml_file_path], ) - return LaunchDescription([ - thruster_interface_node - ]) \ No newline at end of file + return LaunchDescription([thruster_interface_node]) \ No newline at end of file diff --git a/motion/thruster_interface/src/thruster_interface.cpp b/motion/thruster_interface/src/thruster_interface.cpp index 73bef202..c6450d42 100644 --- a/motion/thruster_interface/src/thruster_interface.cpp +++ b/motion/thruster_interface/src/thruster_interface.cpp @@ -17,6 +17,7 @@ void init(int &file) { } else { std::cout << "connected to i2c bus!!" << std::endl; } + if (ioctl(file, I2C_SLAVE, i2c_slave_addr) < 0) { std::cerr << "error, could not set adress" << std::endl; close(file); @@ -32,7 +33,7 @@ void send_status(int8_t status, int file) { if (write(file, &status, 1) != 1) { std::cerr << "error, could not send status" << std::endl; close(file); - // exit(EXIT_FAILURE); + exit(EXIT_FAILURE); } else { std::cout << "status data has been sent" << std::endl; } @@ -40,7 +41,6 @@ void send_status(int8_t status, int file) { void send_pwm(std::vector pwm_values, int file) { std::vector bytes = pwm_to_bytes(pwm_values); - // if(write(file, bytes.data(), bytes.size()) != bytes.size()){ if (static_cast(write(file, bytes.data(), bytes.size())) != bytes.size()) { std::cerr << "error, could not send PWM data" << std::endl; @@ -127,40 +127,42 @@ void get_pwm_table() { file.close(); } -uint16_t interpolate(double force) { - +uint16_t interpolate(double force, int PWM_min, int PWM_max) { + uint16_t pwm; if (pwm_table.empty()) { get_pwm_table(); } - - auto it = pwm_table.lower_bound( - force); // Find the first element with a key not less than force - - // If the force is less than or equal to the smallest force in the table - if (it == pwm_table.begin()) - return it->second; - - // If the force is greater than or equal to the largest force in the table - if (it == pwm_table.end()) { + // Find the first element with a key not less than force + auto it = pwm_table.lower_bound(force); + if (it == pwm_table.begin()){ // If the force is less than or equal to the smallest force in the table + pwm = it->second; + }else if (it == pwm_table.end()) { // If the force is greater than or equal to the largest force in the table --it; - return it->second; - } - + pwm = it->second; + }else{ // Linear interpolation auto prev = std::prev(it); // Get the element with the next smaller key double force1 = prev->first; double force2 = it->first; double pwm1 = prev->second; double pwm2 = it->second; - - return pwm1 + (pwm2 - pwm1) * (force - force1) / (force2 - force1); + pwm = pwm1 + (pwm2 - pwm1) * (force - force1) / (force2 - force1); + } + //check if the calculated pwm values are within the defined limits [PWM_min, PWM_max]. + if (pwm < PWM_min){ + return PWM_min; + }else if(pwm > PWM_max){ + return PWM_max; + }else{ + return pwm; + } } -std::vector interpolate_all(std::vector &force_values) { +std::vector interpolate_all(std::vector &force_values,int PWM_min, int PWM_max) { std::vector interpolatedVector; // Interpolate each value in the input vector for (const auto &force : force_values) { - interpolatedVector.push_back(interpolate(force)); + interpolatedVector.push_back(interpolate(force, PWM_min, PWM_max)); } return interpolatedVector; diff --git a/motion/thruster_interface/src/thruster_interface_node.cpp b/motion/thruster_interface/src/thruster_interface_node.cpp index 2b2152ea..b837a326 100644 --- a/motion/thruster_interface/src/thruster_interface_node.cpp +++ b/motion/thruster_interface/src/thruster_interface_node.cpp @@ -40,8 +40,6 @@ class ThrusterInterface : public rclcpp::Node { "/asv/temperature/ESC3/test", 10); publisher_ESC4 = this->create_publisher( "/asv/temperature/ESC4/test", 10); - publisher_ESC4 = this->create_publisher( - "/asv/temperature/ESC4/test", 10); publisher_ambient1 = this->create_publisher( "/asv/temperature/ambient1/test", 10); publisher_ambient2 = this->create_publisher( @@ -49,8 +47,22 @@ class ThrusterInterface : public rclcpp::Node { publisher_status = this->create_publisher( "/asv/failsafe/hardware/status", 10); + + // get parameters from the freya.yaml file + + this->declare_parameter("thruster_interface.PWM_min", 500); + int PWM_min = this->get_parameter("thruster_interface.PWM_min").as_int(); + this->declare_parameter("thruster_interface.PWM_max", 500); + int PWM_max = this->get_parameter("thruster_interface.PWM_max").as_int(); + this->declare_parameter("thruster_interface.publishing_rate", 500); + int publishing_rate = this->get_parameter("thruster_interface.publishing_rate").as_int(); + + cout << "800 : " << interpolate(800, PWM_min, PWM_max) << endl; + cout << "-10000 : " << interpolate(-10000, PWM_min, PWM_max) << endl; + cout << "publishing rate : " << publishing_rate << endl; + timer_ = this->create_wall_timer( - std::chrono::seconds(1), + std::chrono::milliseconds(publishing_rate), std::bind(&ThrusterInterface::publish_temperature_and_status, this)); }