diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b51eab..83bc107 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -115,8 +115,8 @@ target_link_libraries(stopping Eigen3::Eigen ) -add_executable(compass - src/Compass_Nav.cpp +add_executable(gyro + src/gyro_turn.cpp src/fan_publisher.cpp src/light_publisher.cpp src/ref_speed_publisher.cpp @@ -127,7 +127,7 @@ add_executable(compass src/heading.cpp ) -ament_target_dependencies(compass +ament_target_dependencies(gyro rclcpp wheelchair_sensor_msgs sensor_msgs @@ -135,13 +135,13 @@ ament_target_dependencies(compass pcl_conversions ) -target_include_directories(compass PUBLIC +target_include_directories(gyro PUBLIC ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}/headers ) -target_link_libraries(compass +target_link_libraries(gyro obstacle_publisher Eigen3::Eigen ) @@ -185,7 +185,7 @@ install(TARGETS obstacle_publisher_node obstacle_publisher stopping - compass + gyro temp_monitor DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/Compass_Nav.cpp b/src/Compass_Nav.cpp deleted file mode 100644 index 335ca3f..0000000 --- a/src/Compass_Nav.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/* main.cpp – left-pivot obstacle avoidance - drive_mode: 0=manual, 1=joystick pass-through, 2=autonomous */ - -#include "main.h" -#include "rclcpp/rclcpp.hpp" -#include "sensors_subscriber.hpp" -#include "obstacle_subscriber.hpp" -#include "ref_speed_publisher.hpp" -#include "heading.hpp" -#include -#include -#include - -/* ── config ─────────────────────────────────────────────── */ -constexpr int SPEED = 15; -constexpr int INNER_SPEED = SPEED / 4; // 3 -constexpr double PIVOT_DURATION = 2.5; // s -constexpr double RETURN_PIVOT_DURATION = 2.5; // s - -/* ── shared flags ───────────────────────────────────────── */ -std::atomic_bool front_clear{true}; -std::atomic_bool back_clear{true}; -std::atomic_bool left_turn_clear{true}; // ← we *only* use this one -std::atomic_bool right_turn_clear{true}; // kept for wiring; ignored -std::atomic_int drive_mode{0}; - -/* ── FSM ────────────────────────────────────────────────── */ -enum class Mode { STRAIGHT, PIVOT, RETURN_PIVOT, STOP }; -Mode mode = Mode::STRAIGHT; -double timer = 0.0; -int pivot_dir = -1; // −1 = left (right never used) - -/* ───────────────────────────────────────────────────────── */ -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("wheelchair_code_module"); - - auto sensors_sub = std::make_shared(node); - auto obstacle_sub = std::make_shared( - node, front_clear, back_clear, left_turn_clear, right_turn_clear); - auto ref_pub = std::make_shared(node); - - auto electron_sub = node->create_subscription( - "electron_selfdrive", 10, - [node](std_msgs::msg::Int32::SharedPtr m){ - drive_mode.store(m->data, std::memory_order_relaxed); - RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); - }); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node); - std::thread spin_thread([&]{ exec.spin(); }); - - rclcpp::Rate loop(30); - const double DT = 1.0 / 30.0; - - while (rclcpp::ok()) - { - RefSpeed cmd{SPEED, SPEED}; - - /* ─ mode 1: joystick passthrough ─ */ - if (drive_mode.load() == 1) { - auto s = sensors_sub->get_latest_sensor_data(); - cmd.leftSpeed = s.left_speed; - cmd.rightSpeed = s.right_speed; - - //temp code for the heading - //auto heading_value = heading(s.magnetic_field_x, s.magnetic_field_y); - //RCLCPP_INFO(node->get_logger(), "Heading: %.2f degrees", heading_value); - - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - /* ─ mode 2: autonomous ─ */ - if (drive_mode.load() == 2) { - bool front = front_clear.load(); - bool left_ok = left_turn_clear.load(); - bool right_ok = right_turn_clear.load(); - - switch (mode) - { - case Mode::STRAIGHT: - if (!front) { - if (!left_ok && !right_ok) { - mode = Mode::STOP; // nowhere to go - } else if (left_ok){ - pivot_dir = -1; // always left - timer = 0; - mode = Mode::PIVOT; - } else if(right_ok){ - pivot_dir = 1; // turn right - timer = 0; - mode = Mode::PIVOT; - } - } - break; - - case Mode::PIVOT: - if(pivot_dir == -1){//pivot left - if (!left_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= PIVOT_DURATION) { - pivot_dir = -pivot_dir; // swing back right - timer = 0; - mode = Mode::RETURN_PIVOT; - } - } else if (pivot_dir == 1){ - if(!right_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.rightSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.leftSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= PIVOT_DURATION) { - pivot_dir = -pivot_dir; // swing back left - timer = 0; - mode = Mode::RETURN_PIVOT; - } - } - - break; - - case Mode::RETURN_PIVOT: - if(pivot_dir == 1){ //starting going left, now returning right - if (!left_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.leftSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.rightSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= RETURN_PIVOT_DURATION) { - timer = 0; - mode = front ? Mode::STRAIGHT : Mode::STOP; - } - } else if (pivot_dir == -1){ //starting going right, now returning left - if (!right_ok) { mode = Mode::STOP; break; } - timer += DT; - cmd.rightSpeed = (pivot_dir == +1) ? SPEED : INNER_SPEED; - cmd.leftSpeed = (pivot_dir == +1) ? INNER_SPEED : SPEED; - if (timer >= RETURN_PIVOT_DURATION) { - timer = 0; - mode = front ? Mode::STRAIGHT : Mode::STOP; - } - } - break; - - case Mode::STOP: - cmd.leftSpeed = cmd.rightSpeed = 0; - if (front) mode = Mode::STRAIGHT; - break; - } - - ref_pub->trigger_publish(cmd); - loop.sleep(); - continue; - } - - /* mode 0: manual/off */ - loop.sleep(); - } - - exec.cancel(); - spin_thread.join(); - rclcpp::shutdown(); - return 0; -} diff --git a/src/gyro_turn.cpp b/src/gyro_turn.cpp new file mode 100644 index 0000000..f960649 --- /dev/null +++ b/src/gyro_turn.cpp @@ -0,0 +1,135 @@ +/* main.cpp – left-pivot obstacle avoidance + drive_mode: 0=manual, 1=joystick pass-through, 2=autonomous */ + +#include "main.h" +#include "rclcpp/rclcpp.hpp" +#include "sensors_subscriber.hpp" +#include "obstacle_subscriber.hpp" +#include "ref_speed_publisher.hpp" +#include "heading.hpp" +#include +#include +#include +#include + +/* ── config ─────────────────────────────────────────────── */ +constexpr int SPEED = 15; +constexpr int INNER_SPEED = SPEED / 4; // 3 +constexpr double PIVOT_DURATION = 2.5; // s +constexpr double RETURN_PIVOT_DURATION = 2.5; // s + +/* ── shared flags ───────────────────────────────────────── */ +std::atomic_bool front_clear{true}; +std::atomic_bool back_clear{true}; +std::atomic_bool left_turn_clear{true}; // ← we *only* use this one +std::atomic_bool right_turn_clear{true}; // kept for wiring; ignored +std::atomic_int drive_mode{0}; + +/* ── FSM ────────────────────────────────────────────────── */ +enum class Mode { STRAIGHT, PIVOT, RETURN_PIVOT, STOP }; +Mode mode = Mode::STRAIGHT; +double timer = 0.0; +int pivot_dir = -1; // −1 = left (right never used) + +/* ───────────────────────────────────────────────────────── */ +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("wheelchair_code_module"); + + auto sensors_sub = std::make_shared(node); + auto obstacle_sub = std::make_shared( + node, front_clear, back_clear, left_turn_clear, right_turn_clear); + auto ref_pub = std::make_shared(node); + + auto electron_sub = node->create_subscription( + "electron_selfdrive", 10, + [node](std_msgs::msg::Int32::SharedPtr m){ + drive_mode.store(m->data, std::memory_order_relaxed); + RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); + }); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node); + std::thread spin_thread([&]{ exec.spin(); }); + + rclcpp::Rate loop(30); + + while (rclcpp::ok()) + { + RefSpeed cmd{SPEED, SPEED}; + + /* ─ mode 0: joystick passthrough ─ */ + if (drive_mode.load() == 0) { + auto s = sensors_sub->get_latest_sensor_data(); + cmd.leftSpeed = s.left_speed; + cmd.rightSpeed = s.right_speed; + + //temp code for the heading + //auto heading_value = heading(s.magnetic_field_x, s.magnetic_field_y); + //RCLCPP_INFO(node->get_logger(), "Heading: %.2f degrees", heading_value); + + ref_pub->trigger_publish(cmd); + loop.sleep(); + continue; + } + /* ─ mode 2: autonomous ─ */ + if (drive_mode.load() == 2) { + + const float target_angle = 90.0; + static float curr_angle = 0; + static float prev_error = 0.0f; + static auto prevTime = std::chrono::steady_clock::now(); // initilization + + auto sensorValue = sensors_sub->get_latest_sensor_data(); + float gyro_vel_z = sensorValue.angular_velocity_z * (180.0f / M_PI); + auto currentTime = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = currentTime - prevTime; + double dt = elapsed.count(); // seconds + + curr_angle += gyro_vel_z * dt; + + //PD controller + float error = target_angle - curr_angle; + float derivative = (error - prev_error) / dt; + + const float Kp = 1.0f; + const float Kd = 0.2f; + + float output = Kp * error + Kd * derivative; + + const float maxSpeed = SPEED; + output = std::clamp(output, -maxSpeed, maxSpeed); + + RefSpeed cmd{0, static_cast(output)}; + ref_pub->trigger_publish(cmd); + + RCLCPP_INFO(node->get_logger(), "Angle = %.2f deg, Output speed = %.2f", curr_angle, output); + + prev_error = error; + + // if(abs(curr_angle) < target_angle){ + // RefSpeed cmd{0, output}; + // ref_pub->trigger_publish(cmd); + // RCLCPP_INFO(node->get_logger(), "Turning… angle = %.3f deg / %.3f deg", curr_angle, target_angle); + // } else { + // RefSpeed cmd{0, 0}; + // ref_pub->trigger_publish(cmd); + // RCLCPP_INFO(node->get_logger(), "Reached target angle"); + // } + + prevTime = currentTime; + + loop.sleep(); + continue; + } + + /* mode extra*/ + loop.sleep(); + } + + exec.cancel(); + spin_thread.join(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/main.cpp b/src/main.cpp index 6077c32..784ea08 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,9 +5,9 @@ * 1 : joystick + obstacle brakes * 2 : autonomous explained above */ - + #include "main.h" -#include "uwb_subscriber.hpp" +#include "uwb_subscriber.hpp" #include "rclcpp/rclcpp.hpp" #include "sensors_subscriber.hpp" #include "obstacle_subscriber.hpp" @@ -15,6 +15,7 @@ #include #include #include +#include /* ── constants ─────────────────────────────────────────── */ constexpr int SPEED = 30; @@ -23,8 +24,8 @@ constexpr double PIVOT_TIME = 2.5; constexpr double RETURN_TIME = 2.5; constexpr float UWB_STOP_RANGE = 4.5; // d6 ≤ 2.15 m → hard stop constexpr float UWB_TURN_RANGE = 3.0; // d2 ≤ 3.0 m → spin + FSM -constexpr double TURN90_TIME = 3.2; -constexpr double UWB_STOPPING_FIRST = 1.9; +constexpr double TURN90_TIME = 3.2; +constexpr double UWB_STOPPING_FIRST = 1.9; /* ── shared flags set by subscribers ───────────────────── */ @@ -66,7 +67,7 @@ int main(int argc, char* argv[]) drive_mode.store(m->data, std::memory_order_relaxed); RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); }); - + auto electron_room = node->create_subscription( "electron_room", 10, @@ -75,18 +76,18 @@ int main(int argc, char* argv[]) room.store(m->data, std::memory_order_relaxed); RCLCPP_INFO(node->get_logger(), "[RX] drive-mode=%d", m->data); }); - + /* receive message from UWB sensors */ auto uwb_sub = std::make_shared(node); - + /* spin ROS callbacks in a background thread */ rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(node); + exec.add_node(node); std::thread spin_thread([&]{ exec.spin(); }); - rclcpp::Clock steady_clock{RCL_STEADY_TIME}; - rclcpp::Time spin_start; + rclcpp::Clock steady_clock{RCL_STEADY_TIME}; + rclcpp::Time spin_start; /* main control loop – 30 Hz */ rclcpp::Rate loop(30); @@ -99,19 +100,19 @@ int main(int argc, char* argv[]) RefSpeed cmd{SPEED, SPEED}; /* ---------- drive_mode 0 : raw joystick ---------- */ - + if (sel == 0) { auto j = sensors_sub->get_latest_sensor_data(); cmd.leftSpeed = j.left_speed; cmd.rightSpeed = j.right_speed; - ref_pub->trigger_publish(cmd); - loop.sleep(); + ref_pub->trigger_publish(cmd); + loop.sleep(); continue; } /* ---------- drive_mode 1 : joystick + brakes ----- */ if (sel == 1) { - + auto j = sensors_sub->get_latest_sensor_data(); cmd.leftSpeed = j.left_speed; cmd.rightSpeed = j.right_speed; @@ -128,20 +129,20 @@ int main(int argc, char* argv[]) /* ---------- drive_mode 2 : autonomous ------------ */ if (sel == 2) { - + bool f_ok = front_clear.load(); bool b_ok = !back_clear.load(std::memory_order_relaxed); bool l_ok = left_turn_clear.load(); //float d6 = 0.0; // front beacon float d2 = uwb_sub->dist2(); // left-flank sensor float d3 = uwb_sub->dist3(); // left-flank sensor - + RCLCPP_INFO(node->get_logger(), "UWB2: %f", d2); RCLCPP_INFO(node->get_logger(), "UWB3: %f", d3); - - + + if (room_val == 419 && d3 > UWB_STOPPING_FIRST && turning90 == false){ RefSpeed cmd{SPEED, SPEED}; if (!f_ok && !b_ok) { // both ways blocked @@ -157,16 +158,16 @@ int main(int argc, char* argv[]) loop.sleep(); continue; } - + if (room_val == 419 && d3 > 0 && d3 < UWB_STOPPING_FIRST && turning90 == false) { RefSpeed cmd{0, 0}; ref_pub->trigger_publish(cmd); loop.sleep(); continue; - + } - + /* d2 trigger → one 90° spin (left) then hand off to FSM */ if (room_val == 400 && d2 > 0 && d2 < UWB_TURN_RANGE && turning90 == false) { @@ -176,14 +177,14 @@ int main(int argc, char* argv[]) RCLCPP_INFO(node->get_logger(), "Starting turn"); loop.sleep(); continue; - + } - + if (room_val == 400 && turning90 == false) { - + bool front_ok = front_clear.load(); bool back_ok = !back_clear.load(std::memory_order_relaxed); - + RefSpeed cmd{SPEED, SPEED}; if (!front_ok && !back_ok) { // both ways blocked cmd = {0, 0}; @@ -197,7 +198,7 @@ int main(int argc, char* argv[]) ref_pub->trigger_publish(cmd); loop.sleep(); continue; - + } if(turning90 == true && !after_spin){ @@ -205,34 +206,34 @@ int main(int argc, char* argv[]) RCLCPP_INFO(node->get_logger(), "Turning"); if (elapsed < TURN90_TIME) { RefSpeed cmd{0, SPEED}; - ref_pub->trigger_publish(cmd); + ref_pub->trigger_publish(cmd); RCLCPP_INFO(node->get_logger(), "Turning… elapsed = %.3f s / %.3f s", elapsed, TURN90_TIME); - loop.sleep(); + loop.sleep(); continue; } /* spin finished */ - //turning90 = false; + //turning90 = false; after_spin = true; cmd = {0,0}; ref_pub->trigger_publish(cmd); loop.sleep(); continue; - + } - + /* After the spin, immediately run the complex FSM */ if (after_spin) { float d6 = uwb_sub->dist6(); - + /* d6 beacon → hard stop */ if (d6 > 0.f && d6 < UWB_STOP_RANGE) { cmd = {0,0}; ref_pub->trigger_publish(cmd); loop.sleep(); continue; } - + bool front_ok = front_clear.load(); bool left_ok = left_turn_clear.load(); switch (mode) diff --git a/src/obstacle_publisher.cpp b/src/obstacle_publisher.cpp index 551b687..d57964d 100644 --- a/src/obstacle_publisher.cpp +++ b/src/obstacle_publisher.cpp @@ -38,8 +38,8 @@ constexpr float F_BOX_X_MIN = 0.0f, F_BOX_X_MAX = 0.60f, F_BOX_Y_HALF = 0.75f; constexpr float B_BOX_X_MIN = -0.40f, B_BOX_X_MAX = -0.05f, B_BOX_Y_HALF = 0.75f; /* tunnel “wall warn” thresholds --------------------------------------- */ -constexpr float F_TUNNEL_Y_HALF = 0.50f, F_WALL_WARN = 1.10f; -constexpr float B_TUNNEL_Y_HALF = 0.50f, B_WALL_WARN = -1.10f; +constexpr float F_TUNNEL_Y_HALF = 0.50f, F_WALL_WARN = 2.50f; +constexpr float B_TUNNEL_Y_HALF = 0.50f, B_WALL_WARN = -2.00f; /* *** side‑cone parameters *** --------------------- */ constexpr float CONE_HALF_ANGLE = static_cast(M_PI) * 35.0f / 180.0f; // ±35°