Skip to content

Commit f1173bb

Browse files
jodhsinghnahaljodhsinghnahal
committed
Merge branch 'MoteusStuff' into MoteusStuff2
2 parents 235bc80 + 1694dbb commit f1173bb

File tree

5 files changed

+564
-70
lines changed

5 files changed

+564
-70
lines changed

‎.gitmodules‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,3 +7,6 @@
77
[submodule "src/external_pkgs/Catch2"]
88
path = src/external_pkgs/Catch2
99
url = https://github.com/catchorg/Catch2.git
10+
[submodule "src/arm_hardware_interface/moteus"]
11+
path = src/arm_hardware_interface/moteus
12+
url = https://github.com/mjbots/moteus.git

‎src/arm_hardware_interface/CMakeLists.txt‎

Lines changed: 16 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -20,11 +20,22 @@ include_directories(
2020
${arm_control_INCLUDE_DIRS}
2121
)
2222

23-
23+
add_subdirectory(moteus)
2424
add_library(${PROJECT_NAME} src/armprotocol.cpp include/arm_hardware_interface/ArmSerialProtocol.h)
2525

26-
add_executable(arm_serial_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
27-
ament_target_dependencies(arm_serial_driver rclcpp sensor_msgs serial rover_msgs arm_control)
26+
27+
# For old and new arm
28+
add_executable(serial_arm_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
29+
add_executable(moteus_arm_driver src/MoteusInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
30+
31+
target_compile_definitions(moteus_arm_driver
32+
PRIVATE
33+
BUILDING_NEW_DRIVER=1
34+
)
35+
36+
target_link_libraries(moteus_arm_driver moteus::cpp)
37+
ament_target_dependencies(serial_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
38+
ament_target_dependencies(moteus_arm_driver rclcpp sensor_msgs serial rover_msgs arm_control)
2839

2940
if(BUILD_TESTING)
3041
find_package(ament_lint_auto REQUIRED)
@@ -49,7 +60,8 @@ install(
4960
)
5061

5162
install(TARGETS
52-
arm_serial_driver
63+
serial_arm_driver
64+
moteus_arm_driver
5365
${PROJECT_NAME}
5466
DESTINATION lib/${PROJECT_NAME}
5567
ARCHIVE DESTINATION lib

‎src/arm_hardware_interface/include/ArmSerialInterface.h‎

Lines changed: 72 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,18 @@
1-
#include <rclcpp/rclcpp.hpp>
2-
#include "sensor_msgs/msg/joy.hpp"
3-
#include "sensor_msgs/msg/joint_state.hpp"
41
#include "arm_control/include/armControlParams.h"
2+
#include "sensor_msgs/msg/joint_state.hpp"
3+
#include "sensor_msgs/msg/joy.hpp"
4+
#include <rclcpp/rclcpp.hpp>
55

66
#include "rover_msgs/msg/arm_command.hpp"
7-
#include <thread>
87
#include <chrono>
8+
#include <thread>
99

10-
#include <arm_hardware_interface/ArmSerialProtocol.h>
10+
#ifdef BUILDING_NEW_DRIVER
11+
#include "moteus.h"
12+
using namespace mjbots;
13+
#endif
1114

15+
#include <arm_hardware_interface/ArmSerialProtocol.h>
1216

1317
#include <serial/serial.h>
1418

@@ -21,85 +25,87 @@
2125
#define AXIS_1_DIR 1
2226
#define AXIS_2_DIR 1
2327
#define AXIS_3_DIR 1
24-
#define AXIS_4_DIR 1
28+
#define AXIS_4_DIR 1
2529
#define AXIS_5_DIR 1
2630
#define AXIS_6_DIR 1
2731

2832
using std::string;
2933

3034
class ArmSerial : public rclcpp::Node {
3135
public:
32-
ArmSerial();
36+
ArmSerial();
3337

34-
// float firm
38+
// float firm
3539

36-
rover_msgs::msg::ArmCommand current_arm_status;
37-
38-
// angle_echo.positions.resize(NUM_JOINTS);
39-
// void start_rx() {
40-
// serialRxThread = std::thread(&ArmSerial::serial_rx(), this);
41-
// }
42-
// string joint_names[6] = {"joint_turntable", "joint_axis1", "joint_axis2", "joint_axis3", "joint_axis4", "joint_ender"}; //? old arm urdf
43-
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
40+
rover_msgs::msg::ArmCommand current_arm_status;
4441

42+
// angle_echo.positions.resize(NUM_JOINTS);
43+
// void start_rx() {
44+
// serialRxThread = std::thread(&ArmSerial::serial_rx(), this);
45+
// }
46+
// string joint_names[6] = {"joint_turntable", "joint_axis1", "joint_axis2", "joint_axis3", "joint_axis4", "joint_ender"}; //? old arm
47+
// urdf
48+
string joint_names[NUM_JOINTS + 2] = {"joint_1", "joint_2", "joint_3", "joint_4",
49+
"joint_5", "joint_6", "finger_left_joint", "finger_right_joint"}; //? newest (Sep 2024) arm urdf
4550

4651
private:
52+
unsigned long baud = 115200; // 19200;
53+
string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
54+
55+
serial::Serial teensy;
56+
serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
57+
58+
#ifdef BUILDING_NEW_DRIVER
59+
std::map<int, std::shared_ptr<moteus::Controller>> controllers;
60+
std::map<int, moteus::Query::Result> servo_data;
61+
std::shared_ptr<moteus::Transport> transport;
62+
#endif
63+
bool send_angles = true;
64+
65+
struct Axis {
66+
float curr_pos;
67+
float target_pos;
68+
float speed;
69+
float zero_rad;
70+
float max_rad;
71+
int dir;
72+
};
4773

48-
unsigned long baud = 115200;//19200;
49-
string port = "/dev/serial/by-id/usb-ZEPHYR_UBC_ROVER_Arm_500100C6224069D7-if00";
50-
51-
serial::Serial teensy;
52-
serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
53-
54-
55-
struct Axis{
56-
float curr_pos;
57-
float target_pos;
58-
float speed;
59-
float zero_rad;
60-
float max_rad;
61-
int dir;
62-
};
63-
64-
Axis axes[NUM_JOINTS];
65-
// Axis axis_EE;
66-
67-
float target_position[NUM_JOINTS];
68-
float target_velocities[NUM_JOINTS];
69-
70-
int homed = 0;
71-
bool homing = false;
72-
float EE = 0;
73-
volatile float current_velocity[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
74-
volatile float current_position[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
75-
volatile int current_limit_switches[NUM_JOINTS] = {-1, -1, -1, -1, -1, -1, -1};
76-
77-
rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
78-
void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
74+
Axis axes[NUM_JOINTS];
75+
// Axis axis_EE;
7976

80-
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
77+
float target_position[NUM_JOINTS];
78+
float target_velocities[NUM_JOINTS];
8179

82-
sensor_msgs::msg::JointState prev_joint_states; //for sim
83-
rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
80+
int homed = 0;
81+
bool homing = false;
82+
float EE = 0;
83+
volatile float current_velocity[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
84+
volatile float current_position[NUM_JOINTS] = {00.00, 00.00, 00.00, 00.00, 00.00, 00.00, 00.00};
85+
volatile int current_limit_switches[NUM_JOINTS] = {-1, -1, -1, -1, -1, -1, -1};
8486

85-
rclcpp::TimerBase::SharedPtr timer_;
87+
rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr command_subscriber;
88+
void CommandCallback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
8689

87-
std::thread serialRxThread;
90+
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
8891

89-
void serial_rx(); //? should be static inline?
90-
float degToRad(float deg);
91-
float firmToMoveitOffsetPos(float deg, int axis);
92-
float firmToMoveitOffsetVel(float deg, int axis);
93-
void send_position_command(float pos[NUM_JOINTS]);
94-
void send_velocity_command(float vel[NUM_JOINTS]);
95-
void send_test_limits_command();
96-
void sendHomeCmd(int target_axis);
97-
void sendCommCmd(int target_state);
98-
void sendMsg(std::string outMsg);
99-
void parseArmAngleUart(std::string msg);
100-
void parseLimitSwitchTest(std::string msg);
101-
};
102-
92+
sensor_msgs::msg::JointState prev_joint_states; // for sim
93+
rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_position_publisher;
10394

95+
rclcpp::TimerBase::SharedPtr timer_;
10496

97+
std::thread serialRxThread;
10598

99+
void serial_rx(); //? should be static inline?
100+
float degToRad(float deg);
101+
float firmToMoveitOffsetPos(float deg, int axis);
102+
float firmToMoveitOffsetVel(float deg, int axis);
103+
void send_position_command(float pos[NUM_JOINTS]);
104+
void send_velocity_command(float vel[NUM_JOINTS]);
105+
void send_test_limits_command();
106+
void sendHomeCmd(int target_axis);
107+
void sendCommCmd(int target_state);
108+
void sendMsg(std::string outMsg);
109+
void parseArmAngleUart(std::string msg);
110+
void parseLimitSwitchTest(std::string msg);
111+
};

‎src/arm_hardware_interface/moteus‎

Submodule moteus added at 84a952a

0 commit comments

Comments
 (0)