diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a5d25f..b919a56 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,8 +68,10 @@ add_executable(gravity_comp control/gravity_compasation.cpp) add_executable(comm_test control/openarm_communication_test.cpp) add_executable(unilateral_control control/openarm_unilateral_control.cpp) add_executable(bilateral_control control/openarm_bilateral_control.cpp) +add_executable(joint_sensor_control control/openarm_joint_sensor.cpp) target_link_libraries(gravity_comp PRIVATE openarm_teleop_lib) target_link_libraries(comm_test PRIVATE openarm_teleop_lib) target_link_libraries(unilateral_control PRIVATE openarm_teleop_lib) target_link_libraries(bilateral_control PRIVATE openarm_teleop_lib) +target_link_libraries(joint_sensor_control PRIVATE openarm_teleop_lib) diff --git a/control/gravity_compasation.cpp b/control/gravity_compasation.cpp index d77893a..3d7aab6 100644 --- a/control/gravity_compasation.cpp +++ b/control/gravity_compasation.cpp @@ -37,7 +37,7 @@ int main(int argc, char** argv) { std::signal(SIGINT, signal_handler); std::string arm_side = "right_arm"; - std::string can_interface = "can0"; + std::string can_interface = "can4"; if (argc < 4) { std::cerr << "Usage: " << argv[0] << " " @@ -78,7 +78,7 @@ int main(int argc, char** argv) { openarm::can::socket::OpenArm* openarm = openarm_init::OpenArmInitializer::initialize_openarm(can_interface, true); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); auto start_time = std::chrono::high_resolution_clock::now(); auto last_hz_display = start_time; int frame_count = 0; @@ -133,7 +133,7 @@ int main(int argc, char** argv) { openarm->recv_all(); } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); openarm->disable_all(); openarm->recv_all(); diff --git a/control/openarm_bilateral_control.cpp b/control/openarm_bilateral_control.cpp index a23f3df..1547762 100644 --- a/control/openarm_bilateral_control.cpp +++ b/control/openarm_bilateral_control.cpp @@ -207,8 +207,8 @@ int main(int argc, char **argv) { std::cout << "Root link : " << root_link << std::endl; std::cout << "Leaf link : " << leaf_link << std::endl; - YamlLoader leader_loader("config/leader.yaml"); - YamlLoader follower_loader("config/follower.yaml"); + YamlLoader leader_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/leader.yaml"); + YamlLoader follower_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/follower.yaml"); // Leader parameters std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); @@ -290,7 +290,7 @@ int main(int argc, char **argv) { admin_thread.start_thread(); while (keep_running) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } leader_thread.stop_thread(); diff --git a/control/openarm_joint_sensor.cpp b/control/openarm_joint_sensor.cpp new file mode 100644 index 0000000..a260678 --- /dev/null +++ b/control/openarm_joint_sensor.cpp @@ -0,0 +1,457 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::atomic keep_running(true); +std::atomic sensor_angle_rad(0.0); +std::atomic sensor_velocity_rad_s(0.0); +std::atomic sensor_angle_ready(false); + +constexpr canid_t SENSOR_CAN_ID = 0x01; // TODO: replace with your sensor CAN ID +constexpr double SENSOR_SCALE = 1e-3; // TODO: replace with your sensor scaling +constexpr double SENSOR_OFFSET = 0.0; +constexpr double SENSOR_FILTER_ALPHA = 0.05; // 0~1, larger = less smoothing +constexpr double SENSOR_VEL_FILTER_ALPHA = 0.2; // 0~1, larger = less smoothing + +int open_can_socket(const std::string &can_interface) { + int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (sock < 0) { + throw std::runtime_error("Failed to open CAN socket"); + } + + struct ifreq ifr; + std::snprintf(ifr.ifr_name, IFNAMSIZ, "%s", can_interface.c_str()); + if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) { + close(sock); + throw std::runtime_error("Failed to get CAN interface index"); + } + + struct sockaddr_can addr; + std::memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(sock, reinterpret_cast(&addr), sizeof(addr)) < 0) { + close(sock); + throw std::runtime_error("Failed to bind CAN socket"); + } + + struct can_filter filter; + filter.can_id = SENSOR_CAN_ID; + filter.can_mask = CAN_SFF_MASK; + if (setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) { + close(sock); + throw std::runtime_error("Failed to set CAN filter"); + } + + return sock; +} + +bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) { + struct pollfd pfd; + pfd.fd = sock; + pfd.events = POLLIN; + int ret = poll(&pfd, 1, 10); + if (ret <= 0 || !(pfd.revents & POLLIN)) { + return false; + } + + struct can_frame frame; + int nbytes = read(sock, &frame, sizeof(frame)); + if (nbytes != static_cast(sizeof(frame))) { + return false; + } + + const canid_t frame_id = frame.can_id & CAN_SFF_MASK; + if (frame_id != SENSOR_CAN_ID || frame.can_dlc < 4) { + return false; + } + + int32_t rawPos = 0; + rawPos |= static_cast(frame.data[1]) << 8; + rawPos |= static_cast(frame.data[2]); + int16_t rawVel = 0; + rawVel |= static_cast(frame.data[3]) << 8; + rawVel |= static_cast(frame.data[4]); + + velocity_rad_s_out = (static_cast(rawVel) / 100.0 / 4096.0) * 360.0 * (M_PI / 180.0); // Example conversion, adjust as needed + + angle_rad_out = (static_cast(rawPos) / 4096.0) * 360.0; // Example conversion, adjust as needed + angle_rad_out = angle_rad_out * (M_PI / 180.0) - 3.14; + return true; +} + +void signal_handler(int signal) { + if (signal == SIGINT) { + std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; + keep_running = false; + } +} + +class LeaderArmThread : public PeriodicTimerThread { +public: + LeaderArmThread(std::shared_ptr robot_state, Control *control_l, + double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {} + +protected: + void before_start() override { std::cout << "leader start thread " << std::endl; } + + void after_stop() override { std::cout << "leader stop thread " << std::endl; } + + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + + control_l_->unilateral_step(); + + auto now = std::chrono::steady_clock::now(); + + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + // robot_state_->arm_state().get_response(0).position; // Example: access joint 0 position + + // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; + } + +private: + std::shared_ptr robot_state_; + Control *control_l_; +}; + +class FollowerArmThread : public PeriodicTimerThread { +public: + FollowerArmThread(std::shared_ptr robot_state, Control *control_f, + double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} + +protected: + void before_start() override { std::cout << "follower start thread " << std::endl; } + + void after_stop() override { std::cout << "follower stop thread " << std::endl; } + + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + if (sensor_angle_ready.load()) { + // control_f_->SetArmJointReference(0, 0, 0.01); + control_f_->SetArmJointReference(0, sensor_angle_rad.load(), + sensor_velocity_rad_s.load()); + std::cout << "Sensor angle (rad): " << sensor_angle_rad.load() << std::endl; + std::cout << "Sensor velocity (rad/s): " << sensor_velocity_rad_s.load() << std::endl; + } + // control_f_->SetArmJointReference(0, 0.2, 0.01); // Example: set joint 0 to 0 radians + + control_f_->unilateral_step(); + + auto now = std::chrono::steady_clock::now(); + + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + // std::cout << "Leader joint 0 position: " << robot_state_->arm_state().get_reference(0).position + // << std::endl; + // std::cout << "Leader joint 0 velocity: " << robot_state_->arm_state().get_reference(0).velocity + // << std::endl; + // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; + } + +private: + std::shared_ptr robot_state_; + Control *control_f_; +}; + +class AdminThread : public PeriodicTimerThread { +public: + AdminThread(std::shared_ptr leader_state, + std::shared_ptr follower_state, Control *control_l, + Control *control_f, double hz = 500.0) + : PeriodicTimerThread(hz), + leader_state_(leader_state), + follower_state_(follower_state), + control_l_(control_l), + control_f_(control_f) {} + +protected: + void before_start() override { std::cout << "admin start thread " << std::endl; } + + void after_stop() override { std::cout << "admin stop thread " << std::endl; } + + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); + + // get response + auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); + auto follower_arm_resp = follower_state_->arm_state().get_all_responses(); + + auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); + auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); + + // set referense + leader_state_->arm_state().set_all_references(follower_arm_resp); + leader_state_->hand_state().set_all_references(follower_hand_resp); + + follower_state_->arm_state().set_all_references(leader_arm_resp); + follower_state_->hand_state().set_all_references(leader_hand_resp); + + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + + // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; + } + +private: + std::shared_ptr leader_state_; + std::shared_ptr follower_state_; + Control *control_l_; + Control *control_f_; +}; + +int main(int argc, char **argv) { + try { + std::signal(SIGINT, signal_handler); + + // default configration + std::string arm_side = "right_arm"; + std::string leader_urdf_path; + std::string follower_urdf_path; + std::string leader_can_interface = "can0"; + std::string follower_can_interface = "can2"; + std::string sensor_can_interface = "can0"; + + if (argc < 3) { + std::cerr + << "Usage: " << argv[0] + << " [arm_side] [leader_can] [follower_can]" + " [sensor_can]" + << std::endl; + return 1; + } + + // Required: URDF paths + leader_urdf_path = argv[1]; + follower_urdf_path = argv[2]; + + // Optional: arm_side + if (argc >= 4) { + arm_side = argv[3]; + if (arm_side != "left_arm" && arm_side != "right_arm") { + std::cerr << "[ERROR] Invalid arm_side: " << arm_side + << ". Must be 'left_arm' or 'right_arm'." << std::endl; + return 1; + } + } + + // Optional: CAN interfaces + if (argc >= 6) { + leader_can_interface = argv[4]; + follower_can_interface = argv[5]; + } + + // Optional: sensor CAN interface + if (argc >= 7) { + sensor_can_interface = argv[6]; + } + + // URDF file existence check + if (!std::filesystem::exists(leader_urdf_path)) { + std::cerr << "[ERROR] Leader URDF not found: " << leader_urdf_path << std::endl; + return 1; + } + if (!std::filesystem::exists(follower_urdf_path)) { + std::cerr << "[ERROR] Follower URDF not found: " << follower_urdf_path << std::endl; + return 1; + } + + // Setup dynamics + std::string root_link = "openarm_body_link0"; + std::string leaf_link = + (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; + + // Output confirmation + std::cout << "=== OpenArm Unilateral Control ===" << std::endl; + std::cout << "Arm side : " << arm_side << std::endl; + std::cout << "Leader CAN : " << leader_can_interface << std::endl; + std::cout << "Follower CAN : " << follower_can_interface << std::endl; + std::cout << "Leader URDF path : " << leader_urdf_path << std::endl; + std::cout << "Follower URDF path: " << follower_urdf_path << std::endl; + std::cout << "Root link : " << root_link << std::endl; + std::cout << "Leaf link : " << leaf_link << std::endl; + std::cout << "Sensor CAN : " << sensor_can_interface << std::endl; + + int sensor_can_socket = open_can_socket(sensor_can_interface); + std::thread can_read_thread([&]() { + double filtered_angle = 0.0; + bool filter_initialized = false; + double filtered_velocity = 0.0; + double prev_angle = 0.0; + auto prev_time = std::chrono::steady_clock::now(); + double velocity = 0.0; + while (keep_running) { + double angle = 0.0; + if (read_sensor(sensor_can_socket, angle, velocity)) { + auto now = std::chrono::steady_clock::now(); + double dt = + std::chrono::duration_cast>(now - prev_time) + .count(); + // std::cout << "dt: " << dt << " s, raw angle: " << angle << " rad" << std::endl; + prev_time = now; + + if (!filter_initialized) { + filtered_angle = angle; + filter_initialized = true; + prev_angle = angle; + filtered_velocity = 0.0; + } else { + filtered_angle = SENSOR_FILTER_ALPHA * angle + + (1.0 - SENSOR_FILTER_ALPHA) * filtered_angle; + + if (dt > 1e-6) { + filtered_velocity = SENSOR_VEL_FILTER_ALPHA * velocity + + (1.0 - SENSOR_VEL_FILTER_ALPHA) * + filtered_velocity; + } + prev_angle = angle; + } + sensor_angle_rad.store(filtered_angle); + sensor_velocity_rad_s.store(filtered_velocity); + sensor_angle_ready.store(true); + } + } + }); + + YamlLoader leader_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/leader.yaml"); + YamlLoader follower_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/follower.yaml"); + + // Leader parameters + std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); + std::vector leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); + std::vector leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); + std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); + std::vector leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); + std::vector leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); + + // Follower parameters + std::vector follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); + std::vector follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); + std::vector follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc"); + std::vector follower_k = follower_loader.get_vector("FollowerArmParam", "k"); + std::vector follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv"); + std::vector follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo"); + + Dynamics *leader_arm_dynamics = new Dynamics(leader_urdf_path, root_link, leaf_link); + leader_arm_dynamics->Init(); + + Dynamics *follower_arm_dynamics = new Dynamics(follower_urdf_path, root_link, leaf_link); + follower_arm_dynamics->Init(); + + std::cout << "=== Initializing Leader OpenArm ===" << std::endl; + openarm::can::socket::OpenArm *leader_openarm = + openarm_init::OpenArmInitializer::initialize_openarm(leader_can_interface, true); + + std::cout << "=== Initializing Follower OpenArm ===" << std::endl; + openarm::can::socket::OpenArm *follower_openarm = + openarm_init::OpenArmInitializer::initialize_openarm(follower_can_interface, true); + + size_t leader_arm_motor_num = leader_openarm->get_arm().get_motors().size(); + size_t follower_arm_motor_num = follower_openarm->get_arm().get_motors().size(); + size_t leader_hand_motor_num = leader_openarm->get_gripper().get_motors().size(); + size_t follower_hand_motor_num = follower_openarm->get_gripper().get_motors().size(); + + std::cout << "leader arm motor num : " << leader_arm_motor_num << std::endl; + std::cout << "follower arm motor num : " << follower_arm_motor_num << std::endl; + std::cout << "leader hand motor num : " << leader_hand_motor_num << std::endl; + std::cout << "follower hand motor num : " << follower_hand_motor_num << std::endl; + + // Declare robot_state (Joint and motor counts are assumed to be equal) + std::shared_ptr leader_state = + std::make_shared(leader_arm_motor_num, leader_hand_motor_num); + + std::shared_ptr follower_state = + std::make_shared(follower_arm_motor_num, follower_hand_motor_num); + + Control *control_leader = new Control( + leader_openarm, leader_arm_dynamics, follower_arm_dynamics, leader_state, + 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); + Control *control_follower = + new Control(follower_openarm, leader_arm_dynamics, follower_arm_dynamics, + follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, + follower_arm_motor_num, follower_hand_motor_num); + + control_leader->SetParameter(leader_kp, leader_kd, leader_Fc, leader_k, leader_Fv, + leader_Fo); + + control_follower->SetParameter(follower_kp, follower_kd, follower_Fc, follower_k, + follower_Fv, follower_Fo); + + // set home postion + // std::thread thread_l(&Control::AdjustPosition, control_leader); + // std::thread thread_f(&Control::AdjustPosition, control_follower); + // thread_l.join(); + // thread_f.join(); + + // Start control process + LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY); + FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); + AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, + FREQUENCY); + + leader_thread.start_thread(); + follower_thread.start_thread(); + // admin_thread.start_thread(); + + while (keep_running) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + leader_thread.stop_thread(); + follower_thread.stop_thread(); + admin_thread.stop_thread(); + + leader_openarm->disable_all(); + follower_openarm->disable_all(); + + if (can_read_thread.joinable()) { + can_read_thread.join(); + } + close(sensor_can_socket); + + } catch (const std::exception &e) { + std::cerr << e.what() << '\n'; + } + + return 0; +} \ No newline at end of file diff --git a/control/openarm_unilateral_control.cpp b/control/openarm_unilateral_control.cpp index 622c661..9db2bef 100644 --- a/control/openarm_unilateral_control.cpp +++ b/control/openarm_unilateral_control.cpp @@ -79,7 +79,7 @@ protected: void on_timer() override { static auto prev_time = std::chrono::steady_clock::now(); - + control_f_->unilateral_step(); auto now = std::chrono::steady_clock::now(); @@ -158,7 +158,7 @@ int main(int argc, char **argv) { if (argc < 3) { std::cerr << "Usage: " << argv[0] - << " [arm_side] [leader_can] [follower_can]" + << " [arm_side] [leader_can] [follower_can]" << std::endl; return 1; } @@ -208,8 +208,8 @@ int main(int argc, char **argv) { std::cout << "Root link : " << root_link << std::endl; std::cout << "Leaf link : " << leaf_link << std::endl; - YamlLoader leader_loader("config/leader.yaml"); - YamlLoader follower_loader("config/follower.yaml"); + YamlLoader leader_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/leader.yaml"); + YamlLoader follower_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/follower.yaml"); // Leader parameters std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); diff --git a/script/launch_grav_comp.sh b/script/launch_grav_comp.sh index 7c84ea5..bbf191e 100755 --- a/script/launch_grav_comp.sh +++ b/script/launch_grav_comp.sh @@ -24,7 +24,7 @@ XACRO_FILE="${ARM_TYPE}.urdf.xacro" WS_DIR=~/openarm_ros2_ws XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" URDF_OUT="$TMPDIR/$URDF_NAME" -BIN_PATH=~/openarm_teleop/build/gravity_comp # adjust if needed +BIN_PATH="$WS_DIR/src/openarm_teleop/build/gravity_comp" # adjust if needed # =============================== # Check workspace if [ ! -d "$WS_DIR" ]; then diff --git a/script/launch_unilateral.sh b/script/launch_unilateral.sh index 3c35772..06ec36d 100755 --- a/script/launch_unilateral.sh +++ b/script/launch_unilateral.sh @@ -51,7 +51,7 @@ FOLLOWER_URDF_PATH="$TMPDIR/${ARM_TYPE}_follower.urdf" XACRO_FILE="$ARM_TYPE.urdf.xacro" WS_DIR=~/openarm_ros2_ws XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" -BIN_PATH=~/openarm_teleop/build/unilateral_control +BIN_PATH="$WS_DIR/src/openarm_teleop/build/unilateral_control" # Check workspace if [ ! -d "$WS_DIR" ]; then diff --git a/src/controller/control.cpp b/src/controller/control.cpp index 7120018..1e1b630 100644 --- a/src/controller/control.cpp +++ b/src/controller/control.cpp @@ -90,6 +90,35 @@ void Control::SetParameter(const std::vector& Kp, const std::vectorarm_state().get_size(); + if (index >= arm_size) { + return; + } + + const size_t limit_size = sizeof(position_limit_min_L) / sizeof(position_limit_min_L[0]); + double clamped_position = position_rad; + if (index < limit_size) { + const double min_limit = + (role_ == ROLE_LEADER) ? position_limit_min_L[index] : position_limit_min_F[index]; + const double max_limit = + (role_ == ROLE_LEADER) ? position_limit_max_L[index] : position_limit_max_F[index]; + clamped_position = std::clamp(position_rad, min_limit, max_limit); + } + + JointState target; + target.position = clamped_position; + target.velocity = velocity_rad_s; + target.effort = effort; + + robot_state_->arm_state().set_reference(index, target); +} + bool Control::bilateral_step() { // get motor status std::vector arm_motor_states; diff --git a/src/controller/control.hpp b/src/controller/control.hpp index 79bdb02..3058cf4 100644 --- a/src/controller/control.hpp +++ b/src/controller/control.hpp @@ -80,6 +80,10 @@ public: const std::vector &Fc, const std::vector &k, const std::vector &Fv, const std::vector &Fo); + // Set a single arm joint reference in radians (e.g., sensor follow). + void SetArmJointReference(size_t index, double position_rad, double velocity_rad_s = 0.0, + double effort = 0.0); + bool AdjustPosition(void); // Compute torque based on bilateral diff --git a/src/openarm_constants.hpp b/src/openarm_constants.hpp index 5140436..393edd7 100644 --- a/src/openarm_constants.hpp +++ b/src/openarm_constants.hpp @@ -31,11 +31,11 @@ constexpr double PI = 3.14159265358979323846; #define ROLE_LEADER 1 #define ROLE_FOLLOWER 2 -#define CAN0 "can0" -#define CAN1 "can1" +#define CAN0 "can4" +#define CAN1 "can5" -#define CAN2 "can2" -#define CAN3 "can3" +#define CAN2 "can6" +#define CAN3 "can7" #define TANHFRIC true diff --git a/src/openarm_port/openarm_init.cpp b/src/openarm_port/openarm_init.cpp index 7212894..df837df 100644 --- a/src/openarm_port/openarm_init.cpp +++ b/src/openarm_port/openarm_init.cpp @@ -21,7 +21,7 @@ namespace openarm_init { openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std::string &can_device, bool enable_debug) { MotorConfig config = DEFAULT_MOTOR_CONFIG; - return initialize_openarm(can_device, config, enable_debug); + return initialize_openarm(can_device, config, false); } openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std::string &can_device, @@ -29,10 +29,10 @@ openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std: bool enable_debug) { // Create OpenArm instance openarm::can::socket::OpenArm *openarm = - new openarm::can::socket::OpenArm(can_device, enable_debug); + new openarm::can::socket::OpenArm(can_device, false); // Perform common initialization - initialize_(openarm, config, enable_debug); + initialize_(openarm, config, false); return openarm; }