Moteus stuff2

.gitmodules

@@ -7,3 +7,6 @@
 [submodule "src/external_pkgs/Catch2"]
 	path = src/external_pkgs/Catch2
 	url = https://github.com/catchorg/Catch2.git
+[submodule "src/arm_hardware_interface/moteus"]
+	path = src/arm_hardware_interface/moteus
+	url = https://github.com/mjbots/moteus.git

src/arm_hardware_interface/CMakeLists.txt

@@ -20,11 +20,22 @@ include_directories(
   ${arm_control_INCLUDE_DIRS}
 )
 
-
+add_subdirectory(moteus)
 add_library(${PROJECT_NAME} src/armprotocol.cpp include/arm_hardware_interface/ArmSerialProtocol.h)
 
-add_executable(arm_serial_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
-ament_target_dependencies(arm_serial_driver rclcpp sensor_msgs serial rover_msgs arm_control)
+
+# For old and new arm
+add_executable(serial_arm_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
+add_executable(moteus_arm_driver src/MoteusInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
+
+target_compile_definitions(moteus_arm_driver
+    PRIVATE
+    BUILDING_NEW_DRIVER=1
+)
+
+target_link_libraries(moteus_arm_driver moteus::cpp)
+ament_target_dependencies(serial_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
+ament_target_dependencies(moteus_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -49,7 +60,8 @@ install(
 )
 
 install(TARGETS
-  arm_serial_driver
+  serial_arm_driver
+  moteus_arm_driver
   ${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
   ARCHIVE DESTINATION lib

src/arm_hardware_interface/include/ArmSerialInterface.h

@@ -1,14 +1,18 @@
-#include <rclcpp/rclcpp.hpp>
-#include "sensor_msgs/msg/joy.hpp"
-#include "sensor_msgs/msg/joint_state.hpp"
 #include "arm_control/include/armControlParams.h"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+#include <rclcpp/rclcpp.hpp>
 
 #include "rover_msgs/msg/arm_command.hpp"
-#include <thread>
 #include <chrono>
+#include <thread>
 
-#include <arm_hardware_interface/ArmSerialProtocol.h>
+#ifdef BUILDING_NEW_DRIVER
+#include "moteus.h"
+using namespace mjbots;
+#endif
 
+#include <arm_hardware_interface/ArmSerialProtocol.h>
 
 #include <serial/serial.h>
 
@@ -21,85 +25,87 @@
 #define AXIS_1_DIR 1
 #define AXIS_2_DIR 1
 #define AXIS_3_DIR 1
-#define AXIS_4_DIR 1 
+#define AXIS_4_DIR 1
 #define AXIS_5_DIR 1
 #define AXIS_6_DIR 1
 
 using std::string;
 
 class ArmSerial : public rclcpp::Node {
 public:
-    ArmSerial();
+  ArmSerial();
 
-    // float firm
+  // float firm
 
-   rover_msgs::msg::ArmCommand current_arm_status;
-
-       //   angle_echo.positions.resize(NUM_JOINTS);
-    // void start_rx() {
-    //     serialRxThread = std::thread(&ArmSerial::serial_rx(), this);
-    // }
-    // string joint_names[6] = {"joint_turntable", "joint_axis1", "joint_axis2", "joint_axis3", "joint_axis4", "joint_ender"}; //? old arm urdf
-    string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "finger_left_joint", "finger_right_joint"}; //? newest (Sep 2024) arm urdf
+  rover_msgs::msg::ArmCommand current_arm_status;
 
+  //   angle_echo.positions.resize(NUM_JOINTS);
+  // void start_rx() {
+  //     serialRxThread = std::thread(&ArmSerial::serial_rx(), this);
+  // }
+  // string joint_names[6] = {"joint_turntable", "joint_axis1", "joint_axis2", "joint_axis3", "joint_axis4", "joint_ender"}; //? old arm
+  // urdf
+  string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3",           "joint_4",
+                                        "joint_5", "joint_6", "finger_left_joint", "finger_right_joint"}; //? newest (Sep 2024) arm urdf
 
 private:
+  unsigned long baud = 115200; // 19200;
+  string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
+
+  serial::Serial teensy;
+  serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
+
+#ifdef BUILDING_NEW_DRIVER
+  std::map<int, std::shared_ptr<moteus::Controller>> controllers;
+  std::map<int, moteus::Query::Result> servo_data;
+  std::shared_ptr<moteus::Transport> transport;
+#endif
+  bool send_angles = true;
+
+  struct Axis {
+    float curr_pos;
+    float target_pos;
+    float speed;
+    float zero_rad;
+    float max_rad;
+    int dir;
+  };
 
-    unsigned long baud = 115200;//19200;
-    string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
-
-    serial::Serial teensy;
-    serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
-
-
-    struct Axis{
-      float curr_pos;
-      float target_pos;
-      float speed;
-      float zero_rad;
-      float max_rad;
-      int dir;
-    };
-
-    Axis axes[NUM_JOINTS];
-    // Axis axis_EE;
-
-    float target_position[NUM_JOINTS];
-    float target_velocities[NUM_JOINTS];
-
-    int homed = 0;
-    bool homing = false;
-    float EE = 0;
-    volatile float current_velocity[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
-    volatile float current_position[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
-    volatile int current_limit_switches[NUM_JOINTS] = {-1, -1, -1, -1, -1, -1, -1};
-
-    rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
-    void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
+  Axis axes[NUM_JOINTS];
+  // Axis axis_EE;
 
-    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
+  float target_position[NUM_JOINTS];
+  float target_velocities[NUM_JOINTS];
 
-    sensor_msgs::msg::JointState prev_joint_states; //for sim
-    rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
+  int homed = 0;
+  bool homing = false;
+  float EE = 0;
+  volatile float current_velocity[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
+  volatile float current_position[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
+  volatile int current_limit_switches[NUM_JOINTS] = {-1, -1, -1, -1, -1, -1, -1};
 
-    rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
+  void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
 
-    std::thread serialRxThread;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
 
-    void serial_rx(); //? should be static inline?
-    float degToRad(float deg);
-    float firmToMoveitOffsetPos(float deg, int axis);
-    float firmToMoveitOffsetVel(float deg, int axis);
-    void send_position_command(float pos[NUM_JOINTS]);
-    void send_velocity_command(float vel[NUM_JOINTS]);
-    void send_test_limits_command();
-    void sendHomeCmd(int target_axis);
-    void sendCommCmd(int target_state);
-    void sendMsg(std::string outMsg);
-    void parseArmAngleUart(std::string msg);
-    void parseLimitSwitchTest(std::string msg);
-  };
-
+  sensor_msgs::msg::JointState prev_joint_states; // for sim
+  rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
 
+  rclcpp::TimerBase::SharedPtr timer_;
 
+  std::thread serialRxThread;
 
+  void serial_rx(); //? should be static inline?
+  float degToRad(float deg);
+  float firmToMoveitOffsetPos(float deg, int axis);
+  float firmToMoveitOffsetVel(float deg, int axis);
+  void send_position_command(float pos[NUM_JOINTS]);
+  void send_velocity_command(float vel[NUM_JOINTS]);
+  void send_test_limits_command();
+  void sendHomeCmd(int target_axis);
+  void sendCommCmd(int target_state);
+  void sendMsg(std::string outMsg);
+  void parseArmAngleUart(std::string msg);
+  void parseLimitSwitchTest(std::string msg);
+};