diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cefeef1..a91dc9e 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -38,14 +38,53 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - serial_port_->write(init_command); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::cout << "control_robotic_arm start" << std::endl; serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); serial_port_->write(init_command); - std::cout << "Control robotic arm end" << std::endl; + std::cout << "control_robotic_arm end" << std::endl; +} + +void SlamWrapper::move_to_i() { + const std::vector left_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0xB1, 0x08, 0x6B, 0x07, 0x57, 0x08, 0x4D, 0x0B, 0x89, 0x07, 0xE2, 0x1E, 0xFA + }; + const std::vector default_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + }; + + std::cout << "move_to_i start" << std::endl; + serial_port_->write(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(default_command); + std::cout << "move_to_i end" << std::endl; +} + +void SlamWrapper::move_to_o() { + const std::vector left_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x4D, 0x08, 0xD5, 0x07, 0x5D, 0x08, 0x94, 0x0A, 0xD5, 0x07, 0xFA, 0x05, 0xFA + }; + const std::vector default_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + }; + + std::cout << "move_to_o start" << std::endl; + serial_port_->write(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(default_command); + std::cout << "move_to_o end" << std::endl; } void SlamWrapper::qt_notice_handler(const void *message) { diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cefeef1..a91dc9e 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -38,14 +38,53 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - serial_port_->write(init_command); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::cout << "control_robotic_arm start" << std::endl; serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); serial_port_->write(init_command); - std::cout << "Control robotic arm end" << std::endl; + std::cout << "control_robotic_arm end" << std::endl; +} + +void SlamWrapper::move_to_i() { + const std::vector left_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0xB1, 0x08, 0x6B, 0x07, 0x57, 0x08, 0x4D, 0x0B, 0x89, 0x07, 0xE2, 0x1E, 0xFA + }; + const std::vector default_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + }; + + std::cout << "move_to_i start" << std::endl; + serial_port_->write(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(default_command); + std::cout << "move_to_i end" << std::endl; +} + +void SlamWrapper::move_to_o() { + const std::vector left_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x4D, 0x08, 0xD5, 0x07, 0x5D, 0x08, 0x94, 0x0A, 0xD5, 0x07, 0xFA, 0x05, 0xFA + }; + const std::vector default_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + }; + + std::cout << "move_to_o start" << std::endl; + serial_port_->write(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(default_command); + std::cout << "move_to_o end" << std::endl; } void SlamWrapper::qt_notice_handler(const void *message) { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 94b245d..7e794ce 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -52,8 +52,15 @@ explicit SlamWrapper(); + //test start void control_robotic_arm(); + void move_to_i(); + + void move_to_o(); + + //test end + void qt_notice_handler(const void *message); void odometer_handler(const void *message); diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index cefeef1..a91dc9e 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -38,14 +38,53 @@ 0xFE, 0xFE, 0x0F, 0x3C, 0x0A, 0x40, 0x0A, 0x89, 0x03, 0xDE, 0x08, 0x43, 0x09, 0xA8, 0x07, 0xE1, 0x1E, 0xFA }; - serial_port_->write(init_command); - std::this_thread::sleep_for(std::chrono::seconds(3)); + std::cout << "control_robotic_arm start" << std::endl; serial_port_->write(left_command); std::this_thread::sleep_for(std::chrono::seconds(3)); serial_port_->write(right_command); std::this_thread::sleep_for(std::chrono::seconds(3)); serial_port_->write(init_command); - std::cout << "Control robotic arm end" << std::endl; + std::cout << "control_robotic_arm end" << std::endl; +} + +void SlamWrapper::move_to_i() { + const std::vector left_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x04, 0xFC, 0x08, 0xA2, 0x07, 0x4E, 0x08, 0x5C, 0x0B, 0x61, 0x07, 0xC2, 0x1E, 0xFA + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x0B, 0xB1, 0x08, 0x6B, 0x07, 0x57, 0x08, 0x4D, 0x0B, 0x89, 0x07, 0xE2, 0x1E, 0xFA + }; + const std::vector default_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + }; + + std::cout << "move_to_i start" << std::endl; + serial_port_->write(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(default_command); + std::cout << "move_to_i end" << std::endl; +} + +void SlamWrapper::move_to_o() { + const std::vector left_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x80, 0x08, 0xA1, 0x07, 0x56, 0x08, 0x47, 0x09, 0xD3, 0x08, 0x0F, 0x1E, 0xFA + }; + const std::vector right_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x05, 0x4D, 0x08, 0xD5, 0x07, 0x5D, 0x08, 0x94, 0x0A, 0xD5, 0x07, 0xFA, 0x05, 0xFA + }; + const std::vector default_command = { + 0xFE, 0xFE, 0x0F, 0x3C, 0x07, 0xF7, 0x08, 0x18, 0x08, 0x0B, 0x08, 0x4B, 0x0A, 0x7D, 0x08, 0x0A, 0x1E, 0xFA + }; + + std::cout << "move_to_o start" << std::endl; + serial_port_->write(left_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(right_command); + std::this_thread::sleep_for(std::chrono::seconds(3)); + serial_port_->write(default_command); + std::cout << "move_to_o end" << std::endl; } void SlamWrapper::qt_notice_handler(const void *message) { diff --git a/src/slam_wrapper.hpp b/src/slam_wrapper.hpp index 94b245d..7e794ce 100644 --- a/src/slam_wrapper.hpp +++ b/src/slam_wrapper.hpp @@ -52,8 +52,15 @@ explicit SlamWrapper(); + //test start void control_robotic_arm(); + void move_to_i(); + + void move_to_o(); + + //test end + void qt_notice_handler(const void *message); void odometer_handler(const void *message); diff --git a/src/tcp_client.cpp b/src/tcp_client.cpp index 644f473..709a7dc 100644 --- a/src/tcp_client.cpp +++ b/src/tcp_client.cpp @@ -66,8 +66,11 @@ _slam.delete_all_edge(); _slam.node_name = 0; } else if (received_string == "scan") { - std::cout << "Control robotic arm start" << std::endl; _slam.control_robotic_arm(); + } else if (received_string == "i") { + _slam.move_to_i(); + } else if (received_string == "o") { + _slam.move_to_o(); } else { std::cout << "Unknown command" << std::endl; }