From 93e64a86b7a076bbdc2df4654a172aa18f462ad0 Mon Sep 17 00:00:00 2001 From: Ishan Vermani Date: Thu, 17 Jul 2025 23:02:30 -0600 Subject: [PATCH 1/5] Controller Script --- .../src/rad_science_controller.cpp | 104 ++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100644 ros_ws/src/rad_control/src/rad_science_controller.cpp diff --git a/ros_ws/src/rad_control/src/rad_science_controller.cpp b/ros_ws/src/rad_control/src/rad_science_controller.cpp new file mode 100644 index 00000000..964d9157 --- /dev/null +++ b/ros_ws/src/rad_control/src/rad_science_controller.cpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "custom_interfaces/msg/ca_nraw.hpp" +#include "rad_control/rad.hpp" + +using namespace std::chrono_literals; +using namespace custom_interfaces::msg; +using std::placeholders::_1; + +std::shared_ptr can_config; +std::shared_ptr> can_pub; +std::shared_ptr> can_sub; +uint8_t rad_id, command_id; +bool ack,ready; + +#define INPUT_CHECK(c) try {c} catch(...){RCLCPP_ERROR(can_config->get_logger(), "INVALID INPUT"); continue;} + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + rclcpp::init(argc, argv); + + can_config = std::make_shared("rad_science_controller_node"); + can_pub = can_config->create_publisher("/can/can_out", 10); + + CANraw can_out_msg; + RAD rad{&can_out_msg}; + rclcpp::WallRate loop_rate(500ms); + + std::thread spin_thread([](){rclcpp::spin(can_config);}); + + std::string in; + + std::cout << "Enter Science Arm RAD ID (prefix h for hex #) => "; + std::getline (std::cin,in); + // Removing whitespace + in.erase(std::remove_if(in.begin(), in.end(), isspace), in.end()); + + int base = 10; + INPUT_CHECK( + if (in[0] == 'h') + { + base = 16; + in = in.substr(1); + } + rad_id = std::stoi(in, 0, base); + rad.set_can_id(rad_id); + ) + + while(true) + { + ack = false; + std::cout << "Enter number of steps, prefaced by u (up) or d (down) (e.g. u600). q to quit=> "; + std::getline (std::cin,in); + + in.erase(std::remove_if(in.begin(), in.end(), isspace), in.end()); + if (in == "q" || std::cin.fail()) + break; + + bool up = false; + int steps = 0; + + INPUT_CHECK( + if (in[0] == 'u') + { + up = true; + in = in.substr(1); + } + else if (in[0] == 'd') + { + up = false; + in = in.substr(1); + } + else + { + std::cout << "Invalid input. Exiting"; + break; + } + + steps = std::stoi(in, 0, 10); + + ) + + if (!up) //ADJUST THIS - CORRELATE EITHER UP OR DOWN TO NEGATIVE STEPS + { + steps = -1 * steps; + } + + rad.pulse_stepper((float)steps); + + can_pub->publish(can_out_msg); + RCLCPP_INFO(can_config->get_logger(), "SENT CAN FRAME 0x%x", can_out_msg.address); + + + } + + spin_thread.~thread(); + rclcpp::shutdown(); + +} \ No newline at end of file From fb9e08eb798c4871d55b0db1f8aa2b0be288728f Mon Sep 17 00:00:00 2001 From: Ishan Vermani Date: Thu, 17 Jul 2025 23:20:40 -0600 Subject: [PATCH 2/5] Update cmake lists --- ros_ws/src/rad_control/CMakeLists.txt | 1 + ros_ws/src/rad_control/src/rad_science_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ros_ws/src/rad_control/CMakeLists.txt b/ros_ws/src/rad_control/CMakeLists.txt index e0f9938c..2cc2ade6 100644 --- a/ros_ws/src/rad_control/CMakeLists.txt +++ b/ros_ws/src/rad_control/CMakeLists.txt @@ -18,6 +18,7 @@ set(Targets rad_drive_controller rad_status rad_tool + rad_science_controller rad_calibration_init rover_steer_pos rover_arm_steer_pos diff --git a/ros_ws/src/rad_control/src/rad_science_controller.cpp b/ros_ws/src/rad_control/src/rad_science_controller.cpp index 964d9157..faa4aa01 100644 --- a/ros_ws/src/rad_control/src/rad_science_controller.cpp +++ b/ros_ws/src/rad_control/src/rad_science_controller.cpp @@ -13,7 +13,7 @@ std::shared_ptr can_config; std::shared_ptr> can_pub; std::shared_ptr> can_sub; uint8_t rad_id, command_id; -bool ack,ready; + #define INPUT_CHECK(c) try {c} catch(...){RCLCPP_ERROR(can_config->get_logger(), "INVALID INPUT"); continue;} From 1ccdd76fe9e93f5c3b8a1851c4e013532874c896 Mon Sep 17 00:00:00 2001 From: Ishan Vermani Date: Thu, 17 Jul 2025 23:23:55 -0600 Subject: [PATCH 3/5] compile fixes --- .../src/rad_science_controller.cpp | 40 +++++++++++-------- 1 file changed, 24 insertions(+), 16 deletions(-) diff --git a/ros_ws/src/rad_control/src/rad_science_controller.cpp b/ros_ws/src/rad_control/src/rad_science_controller.cpp index faa4aa01..58a4f852 100644 --- a/ros_ws/src/rad_control/src/rad_science_controller.cpp +++ b/ros_ws/src/rad_control/src/rad_science_controller.cpp @@ -13,6 +13,7 @@ std::shared_ptr can_config; std::shared_ptr> can_pub; std::shared_ptr> can_sub; uint8_t rad_id, command_id; +bool id_assigned = false; #define INPUT_CHECK(c) try {c} catch(...){RCLCPP_ERROR(can_config->get_logger(), "INVALID INPUT"); continue;} @@ -35,25 +36,32 @@ int main(int argc, char ** argv) std::string in; - std::cout << "Enter Science Arm RAD ID (prefix h for hex #) => "; - std::getline (std::cin,in); - // Removing whitespace - in.erase(std::remove_if(in.begin(), in.end(), isspace), in.end()); - - int base = 10; - INPUT_CHECK( - if (in[0] == 'h') - { - base = 16; - in = in.substr(1); - } - rad_id = std::stoi(in, 0, base); - rad.set_can_id(rad_id); - ) + + + while(true) { - ack = false; + + if (!id_assigned) + { + std::cout << "Enter Science Arm RAD ID (prefix h for hex #) => "; + std::getline (std::cin,in); + // Removing whitespace + in.erase(std::remove_if(in.begin(), in.end(), isspace), in.end()); + + INPUT_CHECK( + if (in[0] == 'h') + { + base = 16; + in = in.substr(1); + } + rad_id = std::stoi(in, 0, base); + rad.set_can_id(rad_id); + ) + + id_assigned = true; + } std::cout << "Enter number of steps, prefaced by u (up) or d (down) (e.g. u600). q to quit=> "; std::getline (std::cin,in); From f1d86319ff7064b77146ad77da9d3449828679b5 Mon Sep 17 00:00:00 2001 From: Ishan Vermani Date: Thu, 17 Jul 2025 23:28:34 -0600 Subject: [PATCH 4/5] final compile fix --- ros_ws/src/rad_control/src/rad_science_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ros_ws/src/rad_control/src/rad_science_controller.cpp b/ros_ws/src/rad_control/src/rad_science_controller.cpp index 58a4f852..8042c64b 100644 --- a/ros_ws/src/rad_control/src/rad_science_controller.cpp +++ b/ros_ws/src/rad_control/src/rad_science_controller.cpp @@ -50,13 +50,15 @@ int main(int argc, char ** argv) // Removing whitespace in.erase(std::remove_if(in.begin(), in.end(), isspace), in.end()); + int base = 10; + INPUT_CHECK( if (in[0] == 'h') { base = 16; in = in.substr(1); } - rad_id = std::stoi(in, 0, base); + rad_id = std::stoi(in, 0, 10); rad.set_can_id(rad_id); ) From cd175dd6554942ddd69c73ad411a8f870a4a16a6 Mon Sep 17 00:00:00 2001 From: Ishan Vermani Date: Thu, 17 Jul 2025 23:30:20 -0600 Subject: [PATCH 5/5] complie fix --- ros_ws/src/rad_control/src/rad_science_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_ws/src/rad_control/src/rad_science_controller.cpp b/ros_ws/src/rad_control/src/rad_science_controller.cpp index 8042c64b..58a3058a 100644 --- a/ros_ws/src/rad_control/src/rad_science_controller.cpp +++ b/ros_ws/src/rad_control/src/rad_science_controller.cpp @@ -58,7 +58,7 @@ int main(int argc, char ** argv) base = 16; in = in.substr(1); } - rad_id = std::stoi(in, 0, 10); + rad_id = std::stoi(in, 0, base); rad.set_can_id(rad_id); )