move the control system to inside the absenc node to hopefully reduce…

… latency

robot/rospackages/src/absenc_interface/src/absenc_node.cpp

@@ -1,10 +1,15 @@
 #include "absenc.h"
+#include "math.h"
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include <memory>
 #include <chrono>
+#include <array>
+#include <cmath>
+#include <string>
 /*Include autogenerated message header*/
 #include "absenc_interface/msg/encoder_values.hpp"
+#include <sensor_msgs/msg/joint_state.hpp>
     using namespace std::chrono_literals;
 class AbsEnc : public rclcpp::Node
 {
@@ -21,10 +26,9 @@ class AbsEnc : public rclcpp::Node
       this->declare_parameter("absenc_path", "/dev/ttyUSB0");
       this->declare_parameter("absenc_polling_rate", 100);
 
-      subscription = this->create_subscription<absenc_interface::msg::EncoderValues>(
-      "absenc_values", 10, std::bind(&AbsEnc::encoderValuesTopicCallback, this, std::placeholders::_1));
-
-      publisher = this->create_publisher<absenc_interface::msg::EncoderValues>("absenc_values", 10);
+      angles_publisher = this->create_publisher<absenc_interface::msg::EncoderValues>("absenc_values", 10);
+
+      arm_publisher = this->create_publisher<std_msgs::msg::String>("arm_command", 10);
 
       timer = this->create_wall_timer(
       std::chrono::milliseconds(this->get_parameter("absenc_polling_rate").as_int()), 
@@ -37,6 +41,10 @@ class AbsEnc : public rclcpp::Node
       if(err.error != 0){
         std::cout << "fuic you";
       }
+
+      subscription = this->create_subscription<sensor_msgs::msg::JointState>(
+      "joint_states", 10, std::bind(&AbsEnc::ikValuesCallback, this, std::placeholders::_1));
+
     }
 
   private:
@@ -51,24 +59,76 @@ class AbsEnc : public rclcpp::Node
     ABSENC::PollSlave(2,&absenc_meas_2);
     ABSENC::PollSlave(3,&absenc_meas_3);
 
-    message.angle_1 = absenc_meas_1.angval + 180.f;
+    message.angle_1 = absenc_meas_1.angval < 0 ? absenc_meas_1.angval + 180.f : absenc_meas_1.angval - 180;
     message.angle_2 = absenc_meas_2.angval;
-    message.angle_3 = absenc_meas_3.angval - 180.f;
-
+    message.angle_3 = absenc_meas_3.angval < 0 ? 180 + absenc_meas_3.angval : absenc_meas_3.angval - 180.f;
+
+    // hold onto value for control system
+    abs_angles[1] = message.angle_1;
+    abs_angles[2] = message.angle_2;
+    abs_angles[3] = message.angle_3;
+
     //TODO : Can simplify this a bit. 
-    publisher->publish(message);
+    angles_publisher->publish(message);
   }
 
-    void encoderValuesTopicCallback(const absenc_interface::msg::EncoderValues::SharedPtr msg) const
-    {
-
-      std::cout << "Angle 1 : "  << msg->angle_1 << std::endl;
-      std::cout << "Angle 2 : "  << msg->angle_2 << std::endl;
-      std::cout << "Angle 3 : "  << msg->angle_3 << std::endl;
+    void ikValuesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) {
+      // Convert ik angles to degrees
+      for (int i = 0; i < 4; i++) {
+        ik_angles[i] = msg->position[i] * 180.0 / M_PI;
+      }
+      std::string arm_command = "set_motor_speeds ";
+
+      std::cout << "Angles: ";
+      for (int i = 0; i < std::size(ik_angles); i++) {
+        std::cout << "ik " << ik_angles[i] << " abs " << abs_angles[i] << " ";
+      }
+      std::cout << "\n";
+
+      for (int i = 0; i < std::size(ik_angles); i++) {
+        float absenc_angle = abs_angles[i];
+        float ik_angle = ik_angles[i];
+        float motor_sign = motor_signs[i];
+
+        float absolute_difference = std::abs(absenc_angle - ik_angle);
+        int difference_sign = absenc_angle - ik_angle >= 0 ? 1 : -1;
+
+        if (absolute_difference <= 1) {
+          arm_command += "0 ";
+        } else {
+          // Get motor value from difference from desired position, with gain 2000
+          float value = ((absolute_difference) / 180.0) * 2000.0;
+          // Clamp
+          value = value > 255.0 ? 255.0 : value;
+          // Account for sign differences
+          value *= difference_sign;
+          value *= motor_sign;
+          // Add to msg
+          int intVal = (int)value;
+          arm_command += std::to_string(intVal);
+          arm_command += " ";
+        }
+      }
+
+      // Add 0 values for two last values
+      arm_command += "0 0";
+
+      auto arm_msg = std_msgs::msg::String();
+      arm_msg.data = arm_command;
+
+      arm_publisher->publish(arm_msg);
     }
+
     rclcpp::TimerBase::SharedPtr timer;
-    rclcpp::Subscription<absenc_interface::msg::EncoderValues>::SharedPtr subscription;
-    rclcpp::Publisher<absenc_interface::msg::EncoderValues>::SharedPtr publisher;
+    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription;
+    rclcpp::Publisher<absenc_interface::msg::EncoderValues>::SharedPtr angles_publisher;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr arm_publisher;
+
+    // Both these store 4 angles to be ready if a 4th encoder is added
+    std::array<float, 4> ik_angles = {0.0, 0.0, 0.0, 0.0};
+    std::array<float, 4> abs_angles = {0.0, 0.0, 0.0, 0.0};
+    // Controls if motor sign is aligned with encoder direction
+    std::array<int, 4> motor_signs = {1, -1, -1, -1};
 };
 
 int main(int argc, char * argv[])

robot/rospackages/src/arm_controller/CMakeLists.txt

@@ -0,0 +1,34 @@
+
+cmake_minimum_required(VERSION 3.5)
+project(arm_controller)
+
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(JetsonGPIO)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+       "msg/ArmMotorValues.msg"
+)
+include_directories(include)
+
+add_executable(arm_controller_node src/arm_controller_node.cpp)
+rosidl_target_interfaces(arm_controller_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+ament_target_dependencies(arm_controller_node rclcpp sensor_msgs std_msgs)
+target_link_libraries(arm_controller_node JetsonGPIO::JetsonGPIO)
+
+install(TARGETS
+        arm_controller_node
+        DESTINATION lib/${PROJECT_NAME})
+ament_package()