cleanup and added ros2 parameters for the timer and pwm min/max

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

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<uint16_t> interpolate_all(std::vector<double> &force_values);
+uint16_t interpolate(double force, int PWM_min, int PWM_max);
+std::vector<uint16_t> interpolate_all(std::vector<double> &force_values, int PWM_min = 1100, int PWM_max = 1900);

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
-    ])
+    return LaunchDescription([thruster_interface_node])

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,15 +33,14 @@ 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;
   }
 }
 
 void send_pwm(std::vector<uint16_t> pwm_values, int file) {
   std::vector<uint8_t> bytes = pwm_to_bytes(pwm_values);
-  // if(write(file, bytes.data(), bytes.size()) != bytes.size()){
   if (static_cast<size_t>(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<uint16_t> interpolate_all(std::vector<double> &force_values) {
+std::vector<uint16_t> interpolate_all(std::vector<double> &force_values,int PWM_min, int PWM_max) {
   std::vector<uint16_t> 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;

motion/thruster_interface/src/thruster_interface_node.cpp

@@ -40,17 +40,29 @@ class ThrusterInterface : public rclcpp::Node {
         "/asv/temperature/ESC3/test", 10);
     publisher_ESC4 = this->create_publisher<std_msgs::msg::Float32>(
         "/asv/temperature/ESC4/test", 10);
-    publisher_ESC4 = this->create_publisher<std_msgs::msg::Float32>(
-        "/asv/temperature/ESC4/test", 10);
     publisher_ambient1 = this->create_publisher<std_msgs::msg::Float32>(
         "/asv/temperature/ambient1/test", 10);
     publisher_ambient2 = this->create_publisher<std_msgs::msg::Float32>(
         "/asv/temperature/ambient2/test", 10);
     publisher_status = this->create_publisher<std_msgs::msg::Int8>(
         "/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));
   }