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

@@ -12,52 +12,53 @@ find_package(sensor_msgs REQUIRED)
 find_package(serial REQUIRED)
 find_package(rover_msgs REQUIRED)
 find_package(arm_control REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-include_directories(
-  include
-  ${arm_control_INCLUDE_DIRS}
-)
+# uncomment the following section in order to fill in further dependencies
+# manually. find_package(<dependency> REQUIRED)
+include_directories(include ${arm_control_INCLUDE_DIRS})
 
+add_subdirectory(moteus)
+add_library(${PROJECT_NAME} src/armprotocol.cpp
+                            include/arm_hardware_interface/ArmSerialProtocol.h)
 
-add_library(${PROJECT_NAME} src/armprotocol.cpp include/arm_hardware_interface/ArmSerialProtocol.h)
+# 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/ArmMoteusInterface.h
+                    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)
+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)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
+  # the following line skips the linter which checks for copyrights comment the
+  # line when a copyright and license is added to all source files
   set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
+  # the following line skips cpplint (only works in a git repo) comment the line
+  # when this package is in a git repo and when a copyright and license is added
+  # to all source files
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME}/launch
-)
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/launch)
 
 # Install headers for other packages (for arm serial protocol)
-install(
-  DIRECTORY include/
-  DESTINATION include/
-)
+install(DIRECTORY include/ DESTINATION include/)
 
-install(TARGETS
-  arm_serial_driver
-  ${PROJECT_NAME}
+install(
+  TARGETS serial_arm_driver moteus_arm_driver ${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
   ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-)
+  LIBRARY DESTINATION lib)
 
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 
-
 ament_package()

src/arm_hardware_interface/include/ArmMoteusInterface.h

@@ -0,0 +1,99 @@
+#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 <chrono>
+#include <thread>
+
+#include "moteus.h"
+using namespace mjbots;
+
+#include <arm_hardware_interface/ArmSerialProtocol.h>
+#include <serial/serial.h>
+
+#define SIMULATE false
+#define PI 3.14159
+
+#define TX_UART_BUFF 128
+#define RX_UART_BUFF 128
+
+#define AXIS_1_DIR 1
+#define AXIS_2_DIR 1
+#define AXIS_3_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();
+
+  rover_msgs::msg::ArmCommand current_arm_status;
+
+  string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3",           "joint_4",
+                                        "joint_5", "joint_6", "finger_left_joint", "finger_right_joint"};
+
+private:
+  unsigned long baud = 115200;
+  string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
+
+  serial::Serial teensy;
+  serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000);
+
+  std::map<int, std::shared_ptr<moteus::Controller>> controllers;
+  std::map<int, moteus::Query::Result> servo_data;
+  std::shared_ptr<moteus::Transport> transport;
+
+  bool send_angles = true;
+
+  struct Axis {
+    float curr_pos;
+    float target_pos;
+    float speed;
+    float zero_rad;
+    float max_rad;
+    int dir;
+  };
+
+  Axis axes[NUM_JOINTS];
+
+  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);
+
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
+
+  sensor_msgs::msg::JointState prev_joint_states;
+  rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::thread serialRxThread;
+
+  void serial_rx();
+  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 ConfigureMotor(int axis_number, mjbots::moteus::Controller &controller);
+  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);
+};

src/arm_hardware_interface/include/ArmSerialInterface.h

@@ -1,15 +1,13 @@
-#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>
-
-
 #include <serial/serial.h>
 
 #define SIMULATE false
@@ -21,85 +19,74 @@
 #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();
-
-    // float firm
+  ArmSerial();
 
-   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;
 
+  string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3",           "joint_4",
+                                        "joint_5", "joint_6", "finger_left_joint", "finger_right_joint"};
 
 private:
+  unsigned long baud = 115200;
+  string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
 
-    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
+  serial::Serial teensy;
+  serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000);
 
+  bool send_angles = true;
 
-    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];
+  struct Axis {
+    float curr_pos;
+    float target_pos;
+    float speed;
+    float zero_rad;
+    float max_rad;
+    int dir;
+  };
 
-    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};
+  Axis axes[NUM_JOINTS];
 
-    rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
-    void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
+  float target_position[NUM_JOINTS];
+  float target_velocities[NUM_JOINTS];
 
-    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_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};
 
-    sensor_msgs::msg::JointState prev_joint_states; //for sim
-    rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
+  rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
+  void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
 
-    rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
 
-    std::thread serialRxThread;
+  sensor_msgs::msg::JointState prev_joint_states;
+  rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_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);
-  };
-
+  rclcpp::TimerBase::SharedPtr timer_;
 
+  std::thread serialRxThread;
 
+  void serial_rx();
+  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);
+};