Started working on the ros part. Created a listening node that uses f…

…unctions from the library

motion/oldfiles_deletelater/old_interface.cpp

@@ -0,0 +1,111 @@
+#include <thruster_interface/thruster_interface.hpp>
+
+ThrusterInterface::ThrusterInterface(std::string mapping_file) {
+  // Open the data file
+  std::ifstream data(mapping_file);
+
+  if (!data.is_open()) {
+    std::cerr << "Unable to open file\n";
+    exit(1);
+  }
+
+  std::string line;
+  // Ignore the header line
+  std::getline(data, line);
+
+  while (std::getline(data, line)) {
+    std::istringstream stream(line);
+
+    std::string force_str, pwm_str;
+
+    std::getline(stream, force_str, '\t');
+    std::getline(stream, pwm_str);
+
+    double force = std::stod(force_str);
+    double pwm = std::stod(pwm_str);
+
+    pwm_table[force] = pwm;
+  }
+}
+
+float ThrusterInterface::interpolate(float force) {
+  auto upper_bound = pwm_table.upper_bound(force);
+  if (upper_bound == pwm_table.end()) {
+    --upper_bound;
+  }
+  auto lower_bound = std::prev(upper_bound);
+
+  if (lower_bound == pwm_table.end()) {
+    return upper_bound->second;
+  }
+
+  float force1 = lower_bound->first;
+  float force2 = upper_bound->first;
+
+  float pwm1 = lower_bound->second;
+  float pwm2 = upper_bound->second;
+
+  if (force1 == force2) {
+    return pwm1;
+  }
+
+  int pwm_signal = std::round(
+      pwm1 + ((force - force1) * (pwm2 - pwm1)) / (force2 - force1) + 0.5);
+
+  return pwm_signal;
+}
+
+std::vector<uint8_t>
+ThrusterInterface::pwm_to_bytes(const std::vector<int> &pwm_values) {
+
+  std::vector<uint8_t> bytes;
+  for (const auto &val : pwm_values) {
+    // Ensure the value is in the correct range and cast to an integer
+    int pwm_int = static_cast<int>(
+        std::min(std::max(val, 1400), 1600)); // min 1100, max 1900
+
+    // Split the integer into most significant byte (MSB) and least significant
+    // byte (LSB)
+    uint8_t msb = (pwm_int >> 8) & 0xFF;
+    uint8_t lsb = pwm_int & 0xFF;
+
+    // Add MSB and LSB to the bytes vector
+    bytes.push_back(msb);
+    bytes.push_back(lsb);
+  }
+  return bytes;
+}
+
+void ThrusterInterface::publish_thrust_to_escs(std::vector<double> forces) {
+  std::vector<int> pwm_values;
+  for (auto force : forces) {
+    pwm_values.push_back(interpolate(force));
+  }
+
+  std::vector<uint8_t> pwm_bytes = pwm_to_bytes(pwm_values);
+  int data_size = pwm_bytes.size();
+
+  // DEBUG
+  // for (auto pwm : pwm_values) {
+  //    std::cout << pwm << " ";
+  //}
+  // std::cout << std::endl;
+
+  int file = open(I2C_DEVICE, O_RDWR);
+  if (file < 0) {
+    std::cerr << "Error opening device\n";
+    exit(1);
+  }
+
+  if (ioctl(file, I2C_SLAVE, I2C_ADDRESS) < 0) {
+    std::cerr << "Error setting I2C address\n";
+    exit(1);
+  }
+
+  // Send the I2C message
+  if (write(file, pwm_bytes.data(), data_size) != data_size) {
+    std::cerr << "Error sending data, ignoring message...\n";
+  }
+
+  close(file);
+}

motion/oldfiles_deletelater/old_node.cpp

@@ -0,0 +1,56 @@
+#include <thruster_interface/thruster_interface_node.hpp>
+
+void ThrusterInterfaceROS::thrust_callback(
+    const vortex_msgs::msg::ThrusterForces::SharedPtr msg) {
+  // Convert from Newton to grams
+
+  constexpr double newton_to_gram_conversion_factor = 101.97162;
+
+  std::vector<double> forces_in_grams = {
+      msg->thrust[0] * newton_to_gram_conversion_factor,
+      msg->thrust[1] * newton_to_gram_conversion_factor,
+      msg->thrust[2] * newton_to_gram_conversion_factor,
+      msg->thrust[3] * newton_to_gram_conversion_factor};
+
+  std::vector<int> pwm_values;
+  for (auto force : forces_in_grams) {
+    pwm_values.push_back(thrusterInterface.interpolate(force));
+  }
+
+  vortex_msgs::msg::Pwm pwm_msg;
+  // TODO: Get mapping and offsets from rosparam
+  // Give thrust to thruster 0: publish on pin = thruster_to_pin_map[0]
+  std::vector<int> thruster_to_pin_map = {1, 3, 2, 0};
+  std::vector<int> thruster_direction_map = {1, 1, 1, -1};
+  std::vector<int> pwm_offsets = {100, 100, 100, 100};
+
+  // Iterates through thruster 0 to 3, where 0 is front right, iterated
+  // clockwise
+  for (int i = 0; i < 4; i++) {
+
+    int center_pwm_value = 1500;
+    int offset_from_center_value =
+        pwm_values[i] - center_pwm_value + pwm_offsets[i];
+    int pwm_value_correct_direction =
+        center_pwm_value + thruster_direction_map[i] * offset_from_center_value;
+
+    int pwm_clamped = std::min(std::max(pwm_value_correct_direction, 1400),
+                               1600); // min 1100, max 1900
+    pwm_msg.positive_width_us.push_back(pwm_clamped);
+    pwm_msg.pins.push_back(thruster_to_pin_map[i]);
+  }
+
+  pwm_pub_->publish(pwm_msg);
+
+  thrusterInterface.publish_thrust_to_escs(forces_in_grams);
+}
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto thruster_interface_node = std::make_shared<ThrusterInterfaceROS>();
+  RCLCPP_INFO(thruster_interface_node->get_logger(),
+              "Starting thruster_interface_node");
+  rclcpp::spin(thruster_interface_node);
+  rclcpp::shutdown();
+  return 0;
+}

motion/oldfiles_deletelater/thruster_interface.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <fcntl.h>
+#include <fstream>
+#include <iostream>
+#include <linux/i2c-dev.h>
+#include <map>
+#include <sstream>
+#include <string>
+#include <sys/ioctl.h>
+#include <thread>
+#include <unistd.h>
+#include <vector>
+
+class ThrusterInterface {
+private:
+  std::map<double, double> pwm_table;
+  const int I2C_BUS = 1;
+  const int I2C_ADDRESS = 0x21;
+  const char *I2C_DEVICE = "/dev/i2c-1";
+
+  std::vector<uint8_t> pwm_to_bytes(const std::vector<int> &pwm_values);
+
+public:
+  ThrusterInterface(std::string mapping_file);
+  void publish_thrust_to_escs(std::vector<double> forces);
+  float interpolate(float force);
+};

motion/thruster_interface/include/thruster_interface/thruster_interface.hpp

@@ -1,31 +1,34 @@
-#pragma once
 
-#include <chrono>
-#include <cmath>
-#include <cstdint>
-#include <fcntl.h>
-#include <fstream>
-#include <iostream>
 #include <linux/i2c-dev.h>
-#include <map>
-#include <sstream>
-#include <string>
+#include <iostream>
+#include <fcntl.h>
 #include <sys/ioctl.h>
-#include <thread>
 #include <unistd.h>
 #include <vector>
+#include <cstring> // Include for memcpy
+#include <fstream>
+#include <sstream>
+#include <map>
+#include <cmath>
+
+
+using namespace std;
+
+
+
+//--------------------------INITIALISATION--------------------------
+void init(int &file);
 
-class ThrusterInterface {
-private:
-  std::map<double, double> pwm_table;
-  const int I2C_BUS = 1;
-  const int I2C_ADDRESS = 0x21;
-  const char *I2C_DEVICE = "/dev/i2c-1";
+//--------------------------SENDING--------------------------
+void send_status(int8_t status, int file);
+std::vector<uint8_t> pwm_to_bytes(std::vector<uint16_t> pwm_values);
+void send_pwm(std::vector<uint16_t> pwm_values, int file);
 
-  std::vector<uint8_t> pwm_to_bytes(const std::vector<int> &pwm_values);
+//--------------------------RECEIVING--------------------------
+std::vector<float> readFloatsFromI2C(int file);
+uint8_t read_hardware_statusFromI2C(int file);
 
-public:
-  ThrusterInterface(std::string mapping_file);
-  void publish_thrust_to_escs(std::vector<double> forces);
-  float interpolate(float force);
-};
+//--------------------------INTERPOLATION--------------------------
+void get_pwm_table();
+uint16_t interpolate(float force);
+std::vector<uint16_t> interpolate_all(std::vector<float> &force_values);