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 new file mode 100644 index 00000000..58a3058a --- /dev/null +++ b/ros_ws/src/rad_control/src/rad_science_controller.cpp @@ -0,0 +1,114 @@ +#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 id_assigned = false; + + +#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; + + + + + + while(true) + { + + 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()); + + 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); + ) + + 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); + + 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