diff --git a/pid_control/Makefile b/complete_route/Makefile similarity index 77% rename from pid_control/Makefile rename to complete_route/Makefile index 53f6d94fb..e517b07b9 100644 --- a/pid_control/Makefile +++ b/complete_route/Makefile @@ -1,56 +1,56 @@ -# Compilador e flags +# Compiler and flags CC = g++ CFLAGS = -Wall -Wextra -Werror -std=c++17 -O3 -g -Iincludes `pkg-config --cflags opencv4` -MMD LDFLAGS = -lSDL2 -li2c -lpthread `pkg-config --libs opencv4` -lstdc++fs -lrt -# Diretórios +# Directories SRCDIR = sources APPDIR = apps OBJDIR = build BINDIR = bin -# Fontes por módulo +# Sources by module SRC_TEST_PID = $(APPDIR)/main.cpp $(SRCDIR)/pid_controller.cpp $(SRCDIR)/jetracer.cpp $(SRCDIR)/i2c_device.cpp $(SRCDIR)/computer_vision.cpp -# Objetos gerados +# Generated objects OBJ_TEST_PID = $(patsubst %.cpp,$(OBJDIR)/%.o,$(notdir $(SRC_TEST_PID))) -# Executáveis +# Executables EXEC_TEST_PID = $(BINDIR)/jetracer_pid_controler -# Alvo principal +# Main target all: $(EXEC_TEST_PID) -# Regras para executáveis +# Rules for executables $(EXEC_TEST_PID): $(OBJ_TEST_PID) | $(BINDIR) #$(CC) $(CFLAGS) -o $@ $(addprefix $(OBJDIR)/,$(notdir $^)) -lSDL2 -lpthread `pkg-config --libs opencv4` -lstdc++fs $(CC) $(CFLAGS) -o $@ $(addprefix $(OBJDIR)/,$(notdir $^)) $(LDFLAGS) -# Regra para arquivos objeto +# Rule for object files $(OBJDIR)/%.o: $(SRCDIR)/%.cpp | $(OBJDIR) $(CC) $(CFLAGS) -c $< -o $@ $(OBJDIR)/%.o: $(APPDIR)/%.cpp | $(OBJDIR) $(CC) $(CFLAGS) -c $< -o $@ -# Cria diretório build/ se necessário +# Create build/ directory if needed $(OBJDIR): mkdir -p $(OBJDIR) -# Cria bin/ se necessário +# Create bin/ directory if needed $(BINDIR): mkdir -p $(BINDIR) -# Comandos auxiliares +# Auxiliary commands clean: rm -rf $(OBJDIR) $(BINDIR) re: clean all -# Alvos para rodar específicos +# Targets to run specific executables testpid: $(EXEC_TEST_PID) .PHONY: all clean re control video frames lane frameslane oneframe testpid run -# Inclui dependências geradas automaticamente +# Include automatically generated dependencies -include $(OBJDIR)/*.d diff --git a/complete_route/README.md b/complete_route/README.md new file mode 100644 index 000000000..d5dcf2022 --- /dev/null +++ b/complete_route/README.md @@ -0,0 +1,89 @@ +# PID Control + JetRacer Vision + +C++ implementation of lane detection, PID control, and hardware interface for the JetRacer car, integrated with a Python pipeline that writes camera segmentation masks to shared memory. + +> **Requirements** +> • CMake ≥ 3.18 • OpenCV ≥ 4.5 • SDL2 • Linux I2C (i2c-dev) • Python ≥ 3.9 (for camera_yolo_to_shm.py) + +--- + +## Repository Structure + +apps/ # example executables + └─ main.cpp # PID with joystick + shared memory +includes/jetracer/ # public headers (*.hpp) +sources/ # C++ implementations (*.cpp) +models/ # YOLO / LaneNet model (.pt) +scripts/ # Python utilities +docs/ # Markdown documentation + diagrams +Makefile # default target: make && make run + +--- + +## Main Modules + +| Module | Description | Docs | +| ------------------------ | -------------------------------------------- | ---------------------------------------------------------------------------- | +| `computer_vision` | Lane extraction, center calculation, overlay | [`docs/docs_computer_vision_module.md`](docs/docs_computer_vision_module.md) | +| `pid_controller` | Simplified PID with anti-wind-up | [`docs/docs_pid_module.md`](docs/docs_pid_module.md) | +| `control` (`JetRacer`) | PWM, servo, motors, joystick handling | [`docs/docs_control_module.md`](docs/docs_control_module.md) | +| `hardware` (`I2CDevice`) | RAII wrapper for `/dev/i2c-X` | [`docs/docs_hardware_module.md`](docs/docs_hardware_module.md) | +| `pwm_config` | Advanced PWM frequency and smoothing configs | [`docs/docs_pwm_improvements.md`](docs/docs_pwm_improvements.md) | +| `motor_control` | Advanced motor control and torque management | [`docs/docs_motor_control_improvements.md`](docs/docs_motor_control_improvements.md) | + +--- + +## Execution + +```bash +# Terminal 1: producer writes lane masks to shared memory +python3 scripts/camera_yolo_to_shm.py + +# Terminal 2: run the C++ controller +./bin/jetracer_pid_controler +``` + +## Testing + +```bash +# Compile and test manually +make clean && make +./bin/jetracer_pid_controler +``` + +--- + +## Execution Flow (Simplified) + +```mermaid +graph TD + P1["Python – YOLO mask"] --> M1["/dev/shm/mask_shared"] + M1 --> C1["apps/main.cpp"] + C1 --> V1["computer_vision"] + V1 --> PID1["pid_controller"] + PID1 --> J1["JetRacer::smooth_steering"] +``` + +--- + +## PWM Improvements + +The system now includes advanced PWM improvements to eliminate motor speed pulsation: + +- **Higher PWM Frequency**: Increased from 100Hz to 1000Hz (configurable up to 2000Hz) +- **Speed Smoothing**: Moving average filter with configurable smoothing window +- **Rate Limiting**: Maximum speed change per update to prevent sudden movements +- **Configurable Parameters**: Easy adjustment for different use cases + +## Motor Control Improvements + +Advanced motor control system to eliminate "force to start" sound and improve acceleration: + +- **Intelligent Power Curve**: Exponential power amplification for better low-speed response +- **Minimum PWM Threshold**: Guaranteed initial torque to eliminate startup resistance +- **Torque Amplification**: Boost for low-speed PWM values +- **Smart Acceleration Ramp**: Different rates for acceleration vs. deceleration +- **Emergency Braking**: Intelligent detection and response to sudden changes +- **Deadzone Control**: Eliminates oscillations at very low speeds + +See [`docs/docs_motor_control_improvements.md`](docs/docs_motor_control_improvements.md) for detailed configuration options. diff --git a/pid_control/apps/main.cpp b/complete_route/apps/main.cpp similarity index 72% rename from pid_control/apps/main.cpp rename to complete_route/apps/main.cpp index 3329b4cb3..122d6da66 100644 --- a/pid_control/apps/main.cpp +++ b/complete_route/apps/main.cpp @@ -1,4 +1,3 @@ -// File: sources/main.cpp #include "jetracer/pid_controller.hpp" #include "jetracer/jetracer.hpp" #include "jetracer/computer_vision.hpp" @@ -19,7 +18,8 @@ jetracer::control::JetRacer *jetracer_ptr = nullptr; void signal_handler(int) { - std::cout << std::endl << "[!] Ctrl+C detected. Stopping the JetRacer..." << std::endl; + std::cout << std::endl + << "[!] Ctrl+C detected. Stopping the JetRacer..." << std::endl; if (jetracer_ptr) jetracer_ptr->stop(); std::_Exit(0); @@ -35,6 +35,14 @@ int main() jetracer_ptr = &jetracer; signal(SIGINT, signal_handler); + + jetracer.set_constant_speed_mode(false); + + std::cout << "\n=== JOYSTICK MODE ACTIVATED ===" << std::endl; + std::cout << "Joystick control enabled" << std::endl; + std::cout << "Maximum speed limited to 27% (ideal configuration)" << std::endl; + std::cout << "Use the left joystick to control speed" << std::endl; + jetracer.start(); int shm_fd = shm_open("mask_shared", O_RDWR, 0666); @@ -57,6 +65,12 @@ int main() cv::Mat mask(jetracer::vision::HEIGHT, jetracer::vision::WIDTH, CV_8UC1, img_ptr); int frame_id = 0; + std::cout << "\n=== MAIN SYSTEM RUNNING ===" << std::endl; + std::cout << "Joystick control active" << std::endl; + std::cout << "Maximum speed limited to 27% (ideal configuration)" << std::endl; + std::cout << "Use the left joystick to control speed" << std::endl; + std::cout << "Use Ctrl+C to stop the system at any time" << std::endl; + while (true) { if (flag_ptr[0] != 1) diff --git a/complete_route/bin/jetracer_pid_controler b/complete_route/bin/jetracer_pid_controler new file mode 100755 index 000000000..189f17f05 Binary files /dev/null and b/complete_route/bin/jetracer_pid_controler differ diff --git a/complete_route/build/computer_vision.d b/complete_route/build/computer_vision.d new file mode 100644 index 000000000..8944caecd --- /dev/null +++ b/complete_route/build/computer_vision.d @@ -0,0 +1,113 @@ +build/computer_vision.o: sources/computer_vision.cpp \ + includes/jetracer/computer_vision.hpp \ + /usr/include/opencv4/opencv2/opencv.hpp \ + /usr/include/opencv4/opencv2/opencv_modules.hpp \ + /usr/include/opencv4/opencv2/core.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/core/version.hpp \ + /usr/include/opencv4/opencv2/core/hal/interface.h \ + /usr/include/opencv4/opencv2/core/cv_cpu_dispatch.h \ + /usr/include/opencv4/opencv2/core/base.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.hpp \ + /usr/include/opencv4/opencv2/core/cvstd_wrapper.hpp \ + /usr/include/opencv4/opencv2/core/neon_utils.hpp \ + /usr/include/opencv4/opencv2/core/vsx_utils.hpp \ + /usr/include/opencv4/opencv2/core/check.hpp \ + /usr/include/opencv4/opencv2/core/traits.hpp \ + /usr/include/opencv4/opencv2/core/matx.hpp \ + /usr/include/opencv4/opencv2/core/saturate.hpp \ + /usr/include/opencv4/opencv2/core/fast_math.hpp \ + /usr/include/opencv4/opencv2/core/types.hpp \ + /usr/include/opencv4/opencv2/core/mat.hpp \ + /usr/include/opencv4/opencv2/core/bufferpool.hpp \ + /usr/include/opencv4/opencv2/core/mat.inl.hpp \ + /usr/include/opencv4/opencv2/core/persistence.hpp \ + /usr/include/opencv4/opencv2/core/operations.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.inl.hpp \ + /usr/include/opencv4/opencv2/core/utility.hpp \ + /usr/include/opencv4/opencv2/core/optim.hpp \ + /usr/include/opencv4/opencv2/core/ovx.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/calib3d.hpp \ + /usr/include/opencv4/opencv2/features2d.hpp \ + /usr/include/opencv4/opencv2/flann/miniflann.hpp \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/config.h \ + /usr/include/opencv4/opencv2/core/affine.hpp \ + /usr/include/opencv4/opencv2/dnn.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.hpp \ + /usr/include/opencv4/opencv2/core/async.hpp \ + /usr/include/opencv4/opencv2/dnn/../dnn/version.hpp \ + /usr/include/opencv4/opencv2/dnn/dict.hpp \ + /usr/include/opencv4/opencv2/dnn/layer.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.inl.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/inference_engine.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/../dnn.hpp \ + /usr/include/opencv4/opencv2/flann.hpp \ + /usr/include/opencv4/opencv2/flann/flann_base.hpp \ + /usr/include/opencv4/opencv2/flann/general.h \ + /usr/include/opencv4/opencv2/flann/matrix.h \ + /usr/include/opencv4/opencv2/flann/params.h \ + /usr/include/opencv4/opencv2/flann/any.h \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/saving.h \ + /usr/include/opencv4/opencv2/flann/nn_index.h \ + /usr/include/opencv4/opencv2/flann/result_set.h \ + /usr/include/opencv4/opencv2/flann/all_indices.h \ + /usr/include/opencv4/opencv2/flann/kdtree_index.h \ + /usr/include/opencv4/opencv2/flann/dynamic_bitset.h \ + /usr/include/opencv4/opencv2/flann/dist.h \ + /usr/include/opencv4/opencv2/flann/heap.h \ + /usr/include/opencv4/opencv2/flann/allocator.h \ + /usr/include/opencv4/opencv2/flann/random.h \ + /usr/include/opencv4/opencv2/flann/kdtree_single_index.h \ + /usr/include/opencv4/opencv2/flann/kmeans_index.h \ + /usr/include/opencv4/opencv2/flann/logger.h \ + /usr/include/opencv4/opencv2/flann/composite_index.h \ + /usr/include/opencv4/opencv2/flann/linear_index.h \ + /usr/include/opencv4/opencv2/flann/hierarchical_clustering_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_table.h \ + /usr/include/opencv4/opencv2/flann/autotuned_index.h \ + /usr/include/opencv4/opencv2/flann/ground_truth.h \ + /usr/include/opencv4/opencv2/flann/index_testing.h \ + /usr/include/opencv4/opencv2/flann/timer.h \ + /usr/include/opencv4/opencv2/flann/sampling.h \ + /usr/include/opencv4/opencv2/highgui.hpp \ + /usr/include/opencv4/opencv2/imgcodecs.hpp \ + /usr/include/opencv4/opencv2/videoio.hpp \ + /usr/include/opencv4/opencv2/imgproc.hpp \ + /usr/include/opencv4/opencv2/./imgproc/segmentation.hpp \ + /usr/include/opencv4/opencv2/ml.hpp \ + /usr/include/opencv4/opencv2/ml/ml.inl.hpp \ + /usr/include/opencv4/opencv2/objdetect.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_dictionary.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_board.hpp \ + /usr/include/opencv4/opencv2/objdetect/graphical_code_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/detection_based_tracker.hpp \ + /usr/include/opencv4/opencv2/objdetect/face.hpp \ + /usr/include/opencv4/opencv2/objdetect/charuco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/barcode.hpp \ + /usr/include/opencv4/opencv2/photo.hpp \ + /usr/include/opencv4/opencv2/stitching.hpp \ + /usr/include/opencv4/opencv2/stitching/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/core/cuda.hpp \ + /usr/include/opencv4/opencv2/core/cuda_types.hpp \ + /usr/include/opencv4/opencv2/core/cuda.inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/exposure_compensate.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/seam_finders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/blenders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/video.hpp \ + /usr/include/opencv4/opencv2/video/tracking.hpp \ + /usr/include/opencv4/opencv2/video/background_segm.hpp diff --git a/complete_route/build/computer_vision.o b/complete_route/build/computer_vision.o new file mode 100644 index 000000000..9638b1397 Binary files /dev/null and b/complete_route/build/computer_vision.o differ diff --git a/complete_route/build/i2c_device.d b/complete_route/build/i2c_device.d new file mode 100644 index 000000000..9b1de98c4 --- /dev/null +++ b/complete_route/build/i2c_device.d @@ -0,0 +1,2 @@ +build/i2c_device.o: sources/i2c_device.cpp \ + includes/jetracer/i2c_device.hpp diff --git a/complete_route/build/i2c_device.o b/complete_route/build/i2c_device.o new file mode 100644 index 000000000..b7be20044 Binary files /dev/null and b/complete_route/build/i2c_device.o differ diff --git a/complete_route/build/jetracer.d b/complete_route/build/jetracer.d new file mode 100644 index 000000000..e0225d23e --- /dev/null +++ b/complete_route/build/jetracer.d @@ -0,0 +1,3 @@ +build/jetracer.o: sources/jetracer.cpp includes/jetracer/jetracer.hpp \ + includes/jetracer/i2c_device.hpp includes/jetracer/pwm_config.hpp \ + includes/jetracer/motor_control.hpp diff --git a/complete_route/build/jetracer.o b/complete_route/build/jetracer.o new file mode 100644 index 000000000..5a58b64ce Binary files /dev/null and b/complete_route/build/jetracer.o differ diff --git a/complete_route/build/main.d b/complete_route/build/main.d new file mode 100644 index 000000000..66148e6e6 --- /dev/null +++ b/complete_route/build/main.d @@ -0,0 +1,115 @@ +build/main.o: apps/main.cpp includes/jetracer/pid_controller.hpp \ + /usr/include/opencv4/opencv2/opencv.hpp \ + /usr/include/opencv4/opencv2/opencv_modules.hpp \ + /usr/include/opencv4/opencv2/core.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/core/version.hpp \ + /usr/include/opencv4/opencv2/core/hal/interface.h \ + /usr/include/opencv4/opencv2/core/cv_cpu_dispatch.h \ + /usr/include/opencv4/opencv2/core/base.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.hpp \ + /usr/include/opencv4/opencv2/core/cvstd_wrapper.hpp \ + /usr/include/opencv4/opencv2/core/neon_utils.hpp \ + /usr/include/opencv4/opencv2/core/vsx_utils.hpp \ + /usr/include/opencv4/opencv2/core/check.hpp \ + /usr/include/opencv4/opencv2/core/traits.hpp \ + /usr/include/opencv4/opencv2/core/matx.hpp \ + /usr/include/opencv4/opencv2/core/saturate.hpp \ + /usr/include/opencv4/opencv2/core/fast_math.hpp \ + /usr/include/opencv4/opencv2/core/types.hpp \ + /usr/include/opencv4/opencv2/core/mat.hpp \ + /usr/include/opencv4/opencv2/core/bufferpool.hpp \ + /usr/include/opencv4/opencv2/core/mat.inl.hpp \ + /usr/include/opencv4/opencv2/core/persistence.hpp \ + /usr/include/opencv4/opencv2/core/operations.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.inl.hpp \ + /usr/include/opencv4/opencv2/core/utility.hpp \ + /usr/include/opencv4/opencv2/core/optim.hpp \ + /usr/include/opencv4/opencv2/core/ovx.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/calib3d.hpp \ + /usr/include/opencv4/opencv2/features2d.hpp \ + /usr/include/opencv4/opencv2/flann/miniflann.hpp \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/config.h \ + /usr/include/opencv4/opencv2/core/affine.hpp \ + /usr/include/opencv4/opencv2/dnn.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.hpp \ + /usr/include/opencv4/opencv2/core/async.hpp \ + /usr/include/opencv4/opencv2/dnn/../dnn/version.hpp \ + /usr/include/opencv4/opencv2/dnn/dict.hpp \ + /usr/include/opencv4/opencv2/dnn/layer.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.inl.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/inference_engine.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/../dnn.hpp \ + /usr/include/opencv4/opencv2/flann.hpp \ + /usr/include/opencv4/opencv2/flann/flann_base.hpp \ + /usr/include/opencv4/opencv2/flann/general.h \ + /usr/include/opencv4/opencv2/flann/matrix.h \ + /usr/include/opencv4/opencv2/flann/params.h \ + /usr/include/opencv4/opencv2/flann/any.h \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/saving.h \ + /usr/include/opencv4/opencv2/flann/nn_index.h \ + /usr/include/opencv4/opencv2/flann/result_set.h \ + /usr/include/opencv4/opencv2/flann/all_indices.h \ + /usr/include/opencv4/opencv2/flann/kdtree_index.h \ + /usr/include/opencv4/opencv2/flann/dynamic_bitset.h \ + /usr/include/opencv4/opencv2/flann/dist.h \ + /usr/include/opencv4/opencv2/flann/heap.h \ + /usr/include/opencv4/opencv2/flann/allocator.h \ + /usr/include/opencv4/opencv2/flann/random.h \ + /usr/include/opencv4/opencv2/flann/kdtree_single_index.h \ + /usr/include/opencv4/opencv2/flann/kmeans_index.h \ + /usr/include/opencv4/opencv2/flann/logger.h \ + /usr/include/opencv4/opencv2/flann/composite_index.h \ + /usr/include/opencv4/opencv2/flann/linear_index.h \ + /usr/include/opencv4/opencv2/flann/hierarchical_clustering_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_table.h \ + /usr/include/opencv4/opencv2/flann/autotuned_index.h \ + /usr/include/opencv4/opencv2/flann/ground_truth.h \ + /usr/include/opencv4/opencv2/flann/index_testing.h \ + /usr/include/opencv4/opencv2/flann/timer.h \ + /usr/include/opencv4/opencv2/flann/sampling.h \ + /usr/include/opencv4/opencv2/highgui.hpp \ + /usr/include/opencv4/opencv2/imgcodecs.hpp \ + /usr/include/opencv4/opencv2/videoio.hpp \ + /usr/include/opencv4/opencv2/imgproc.hpp \ + /usr/include/opencv4/opencv2/./imgproc/segmentation.hpp \ + /usr/include/opencv4/opencv2/ml.hpp \ + /usr/include/opencv4/opencv2/ml/ml.inl.hpp \ + /usr/include/opencv4/opencv2/objdetect.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_dictionary.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_board.hpp \ + /usr/include/opencv4/opencv2/objdetect/graphical_code_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/detection_based_tracker.hpp \ + /usr/include/opencv4/opencv2/objdetect/face.hpp \ + /usr/include/opencv4/opencv2/objdetect/charuco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/barcode.hpp \ + /usr/include/opencv4/opencv2/photo.hpp \ + /usr/include/opencv4/opencv2/stitching.hpp \ + /usr/include/opencv4/opencv2/stitching/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/core/cuda.hpp \ + /usr/include/opencv4/opencv2/core/cuda_types.hpp \ + /usr/include/opencv4/opencv2/core/cuda.inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/exposure_compensate.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/seam_finders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/blenders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/video.hpp \ + /usr/include/opencv4/opencv2/video/tracking.hpp \ + /usr/include/opencv4/opencv2/video/background_segm.hpp \ + includes/jetracer/jetracer.hpp includes/jetracer/i2c_device.hpp \ + includes/jetracer/pwm_config.hpp includes/jetracer/motor_control.hpp \ + includes/jetracer/computer_vision.hpp diff --git a/complete_route/build/main.o b/complete_route/build/main.o new file mode 100644 index 000000000..2fe921554 Binary files /dev/null and b/complete_route/build/main.o differ diff --git a/complete_route/build/pid_controller.d b/complete_route/build/pid_controller.d new file mode 100644 index 000000000..28eaa6813 --- /dev/null +++ b/complete_route/build/pid_controller.d @@ -0,0 +1,114 @@ +build/pid_controller.o: sources/pid_controller.cpp \ + includes/jetracer/pid_controller.hpp \ + /usr/include/opencv4/opencv2/opencv.hpp \ + /usr/include/opencv4/opencv2/opencv_modules.hpp \ + /usr/include/opencv4/opencv2/core.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/core/version.hpp \ + /usr/include/opencv4/opencv2/core/hal/interface.h \ + /usr/include/opencv4/opencv2/core/cv_cpu_dispatch.h \ + /usr/include/opencv4/opencv2/core/base.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.hpp \ + /usr/include/opencv4/opencv2/core/cvstd_wrapper.hpp \ + /usr/include/opencv4/opencv2/core/neon_utils.hpp \ + /usr/include/opencv4/opencv2/core/vsx_utils.hpp \ + /usr/include/opencv4/opencv2/core/check.hpp \ + /usr/include/opencv4/opencv2/core/traits.hpp \ + /usr/include/opencv4/opencv2/core/matx.hpp \ + /usr/include/opencv4/opencv2/core/saturate.hpp \ + /usr/include/opencv4/opencv2/core/fast_math.hpp \ + /usr/include/opencv4/opencv2/core/types.hpp \ + /usr/include/opencv4/opencv2/core/mat.hpp \ + /usr/include/opencv4/opencv2/core/bufferpool.hpp \ + /usr/include/opencv4/opencv2/core/mat.inl.hpp \ + /usr/include/opencv4/opencv2/core/persistence.hpp \ + /usr/include/opencv4/opencv2/core/operations.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.inl.hpp \ + /usr/include/opencv4/opencv2/core/utility.hpp \ + /usr/include/opencv4/opencv2/core/optim.hpp \ + /usr/include/opencv4/opencv2/core/ovx.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/calib3d.hpp \ + /usr/include/opencv4/opencv2/features2d.hpp \ + /usr/include/opencv4/opencv2/flann/miniflann.hpp \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/config.h \ + /usr/include/opencv4/opencv2/core/affine.hpp \ + /usr/include/opencv4/opencv2/dnn.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.hpp \ + /usr/include/opencv4/opencv2/core/async.hpp \ + /usr/include/opencv4/opencv2/dnn/../dnn/version.hpp \ + /usr/include/opencv4/opencv2/dnn/dict.hpp \ + /usr/include/opencv4/opencv2/dnn/layer.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.inl.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/inference_engine.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/../dnn.hpp \ + /usr/include/opencv4/opencv2/flann.hpp \ + /usr/include/opencv4/opencv2/flann/flann_base.hpp \ + /usr/include/opencv4/opencv2/flann/general.h \ + /usr/include/opencv4/opencv2/flann/matrix.h \ + /usr/include/opencv4/opencv2/flann/params.h \ + /usr/include/opencv4/opencv2/flann/any.h \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/saving.h \ + /usr/include/opencv4/opencv2/flann/nn_index.h \ + /usr/include/opencv4/opencv2/flann/result_set.h \ + /usr/include/opencv4/opencv2/flann/all_indices.h \ + /usr/include/opencv4/opencv2/flann/kdtree_index.h \ + /usr/include/opencv4/opencv2/flann/dynamic_bitset.h \ + /usr/include/opencv4/opencv2/flann/dist.h \ + /usr/include/opencv4/opencv2/flann/heap.h \ + /usr/include/opencv4/opencv2/flann/allocator.h \ + /usr/include/opencv4/opencv2/flann/random.h \ + /usr/include/opencv4/opencv2/flann/kdtree_single_index.h \ + /usr/include/opencv4/opencv2/flann/kmeans_index.h \ + /usr/include/opencv4/opencv2/flann/logger.h \ + /usr/include/opencv4/opencv2/flann/composite_index.h \ + /usr/include/opencv4/opencv2/flann/linear_index.h \ + /usr/include/opencv4/opencv2/flann/hierarchical_clustering_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_table.h \ + /usr/include/opencv4/opencv2/flann/autotuned_index.h \ + /usr/include/opencv4/opencv2/flann/ground_truth.h \ + /usr/include/opencv4/opencv2/flann/index_testing.h \ + /usr/include/opencv4/opencv2/flann/timer.h \ + /usr/include/opencv4/opencv2/flann/sampling.h \ + /usr/include/opencv4/opencv2/highgui.hpp \ + /usr/include/opencv4/opencv2/imgcodecs.hpp \ + /usr/include/opencv4/opencv2/videoio.hpp \ + /usr/include/opencv4/opencv2/imgproc.hpp \ + /usr/include/opencv4/opencv2/./imgproc/segmentation.hpp \ + /usr/include/opencv4/opencv2/ml.hpp \ + /usr/include/opencv4/opencv2/ml/ml.inl.hpp \ + /usr/include/opencv4/opencv2/objdetect.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_dictionary.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_board.hpp \ + /usr/include/opencv4/opencv2/objdetect/graphical_code_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/detection_based_tracker.hpp \ + /usr/include/opencv4/opencv2/objdetect/face.hpp \ + /usr/include/opencv4/opencv2/objdetect/charuco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/barcode.hpp \ + /usr/include/opencv4/opencv2/photo.hpp \ + /usr/include/opencv4/opencv2/stitching.hpp \ + /usr/include/opencv4/opencv2/stitching/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/core/cuda.hpp \ + /usr/include/opencv4/opencv2/core/cuda_types.hpp \ + /usr/include/opencv4/opencv2/core/cuda.inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/exposure_compensate.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/seam_finders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/blenders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/video.hpp \ + /usr/include/opencv4/opencv2/video/tracking.hpp \ + /usr/include/opencv4/opencv2/video/background_segm.hpp \ + includes/jetracer/computer_vision.hpp diff --git a/complete_route/build/pid_controller.o b/complete_route/build/pid_controller.o new file mode 100644 index 000000000..c82df968c Binary files /dev/null and b/complete_route/build/pid_controller.o differ diff --git a/complete_route/docs/buttons_references.md b/complete_route/docs/buttons_references.md new file mode 100644 index 000000000..f657d4137 --- /dev/null +++ b/complete_route/docs/buttons_references.md @@ -0,0 +1,118 @@ +# JetRacer Joystick Controls Reference + +This document provides a complete reference of joystick control mappings used in the JetRacer system, including analog axes, buttons, and their respective system identifications. + +## Overview + +The JetRacer system uses a standard USB joystick for manual vehicle control. Controls are mapped through the Linux input system (`/dev/input/js0`) and include: + +- **Analog axes**: For steering and speed control +- **Digital buttons**: For specific functions and emergency +- **D-pad**: For menu navigation + +## Analog Axes Mapping + +### D-Pad (Directional) +| Direction | Axis | Value | Description | +|-----------|------|-------|-------------| +| Up | 7 | < 0 | Upward movement | +| Down | 7 | > 0 | Downward movement | +| Left | 6 | < 0 | Leftward movement | +| Right | 6 | > 0 | Rightward movement | + +### Right Stick (Right Thumbstick) +| Direction | Axis | Value | Description | +|-----------|------|-------|-------------| +| Up | 3 | < 0 | Positive vertical movement | +| Down | 3 | > 0 | Negative vertical movement | +| Left | 2 | < 0 | Negative horizontal movement | +| Right | 2 | > 0 | Positive horizontal movement | + +### Left Stick (Left Thumbstick) +| Direction | Axis | Value | Description | +|-----------|------|-------|-------------| +| Up | 1 | < 0 | Positive vertical movement | +| Down | 1 | > 0 | Negative vertical movement | +| Left | 0 | < 0 | Negative horizontal movement | +| Right | 0 | > 0 | Positive horizontal movement | + +## Button Mapping + +### Shoulder Buttons +| Button | ID | State | Description | +|--------|----|-------|-------------| +| R1 | 7 | ON | Right upper button | +| R2 | 9 | ON | Right trigger | +| L1 | 6 | ON | Left upper button | +| L2 | 8 | ON | Left trigger | + +### Action Buttons +| Button | ID | State | Description | +|--------|----|-------|-------------| +| Y | 4 | ON | Y button (yellow) | +| X | 3 | ON | X button (blue) | +| B | 1 | ON | B button (red) | +| A | 0 | ON | A button (green) | + +## Usage in JetRacer System + +### Main Controls +- **Left Stick (Axis 0)**: Vehicle steering control +- **Right Stick (Axis 2)**: Speed/acceleration control +- **A Button**: Start/stop system +- **B Button**: Emergency mode +- **X Button**: Toggle test mode +- **Y Button**: Reset settings + +### Input Values +- **Negative values (< 0)**: Movement in one direction +- **Positive values (> 0)**: Movement in opposite direction +- **Zero value (0)**: Neutral/center position + +## Testing and Verification + +### Test Commands +```bash +# Test connected joystick +jstest /dev/input/js0 + +# Check input devices +ls /dev/input/ + +# Monitor events in real time +cat /dev/input/js0 +``` + +### Online Tools +- **Hardware Tester**: https://hardwaretester.com/gamepad +- **Gamepad Tester**: https://gamepad-tester.com/ + +## Troubleshooting + +### Common Issues +1. **Joystick not detected**: Check USB connection and drivers +2. **Controls not responding**: Check device access permissions +3. **Incorrect values**: Calibrate joystick using `jscal` + +### Diagnostic Commands +```bash +# Check permissions +ls -la /dev/input/js0 + +# Test connectivity +sudo jstest /dev/input/js0 + +# Check events +sudo cat /dev/input/js0 +``` + +## References + +- **Linux Joystick API**: Official Linux kernel documentation +- **jstest**: Joystick testing tool +- **Hardware Tester**: https://hardwaretester.com/gamepad + +--- + +**Last updated**: January 2025 +**Version**: 1.0 diff --git a/complete_route/docs/change_to_model_202507181755.md b/complete_route/docs/change_to_model_202507181755.md new file mode 100644 index 000000000..61ee321a2 --- /dev/null +++ b/complete_route/docs/change_to_model_202507181755.md @@ -0,0 +1,153 @@ +# Migration to New YOLO Model - best_202507181755.pt + +## **Summary of Changes** + +This document describes the changes made to migrate from the previous model to the new model `models/best_202507181755.pt`. + +## **New Model Information** + +### **Technical Characteristics** +- **File**: `models/best_202507181755.pt` +- **Type**: YOLOv8s-seg (segmentation) +- **Parameters**: 11,793,192 +- **GFLOPs**: 42.7 +- **Number of classes**: 8 + +### **Available Classes** +| ID | Class Name | Description | +|----|------------|-------------| +| 0 | drivable | Drivable area | +| 1 | **lane** | **Road lane** | +| 2 | passadeira | Pedestrian crossing | +| 3 | stop sign | Stop sign | +| 4 | speed 50 | Speed limit 50 | +| 5 | speed 80 | Speed limit 80 | +| 6 | jetracer | JetRacer car | +| 7 | gate | Gate | + +## **Changes Made** + +### **1. Camera Script (`scripts/camera_yolo_to_shm.py`)** +```python +# BEFORE: +MODEL_PATH = "models/best.pt" +LANE_CLASS_ID = 80 + +# AFTER: +MODEL_PATH = "models/best_202507181755.pt" +LANE_CLASS_ID = 1 # 'lane' class in new model +``` + +### **2. Information Extraction Script (`scripts/extract_info_yolo_model.py`)** +```python +# BEFORE: +model_path = '/home/jetson/models_/best_202507181755.pt' + +# AFTER: +model_path = '/home/jetson/Documents/e-codes/pid_final/pid_control/models/best_202507181755.pt' +``` + +## **Verifications Performed** + +### **Loading Test** +- Model loads without errors +- Classes are recognized correctly +- 'lane' class is available with ID 1 +- Predictions work normally + +### **Compatibility** +- Same input structure (images) +- Same output structure (masks) +- Compatible with shared memory system +- Compatible with GStreamer pipeline + +## **How to Use the New Model** + +### **1. Run Complete System** +```bash +# Terminal 1: Start camera script +python3 scripts/camera_yolo_to_shm.py + +# Terminal 2: Start PID control +make clean && make +./bin/jetracer_pid_controler +``` + +### **2. Test Model Only** +```bash +# Check model information +python3 scripts/extract_info_yolo_model.py +``` + +## **Recommended Settings** + +### **For Camera Script** +```python +MODEL_PATH = "models/best_202507181755.pt" +LANE_CLASS_ID = 1 # 'lane' class +CONF_THRESHOLD = 0.25 # Can be adjusted as needed +``` + +### **Model Training Parameters** +- **Epochs**: 150 +- **Batch size**: 16 +- **Image size**: 320x320 +- **Optimizer**: auto +- **Learning rate**: 0.002 + +## **Main Differences** + +### **Previous vs. New Model** +| Aspect | Previous Model | New Model | +|--------|----------------|-----------| +| File name | `best.pt` | `best_202507181755.pt` | +| Lane class ID | 80 | 1 | +| Number of classes | Unknown | 8 | +| Training date | Unknown | 18/07/2025 | + +## **Important Considerations** + +### **1. Previous Model Backup** +- The previous model (`best.pt`) was not removed +- Can be restored by changing `MODEL_PATH` back + +### **2. Performance** +- The new model may have different performance +- Adjust `CONF_THRESHOLD` if necessary +- Monitor detection quality + +### **3. Additional Classes** +- The new model detects more classes (8 vs. previous) +- This may improve system robustness +- Classes like 'stop sign' and 'speed' may be useful in the future + +## **Recommended Tests** + +### **1. Basic Test** +- Verify system starts without errors +- Confirm masks are generated +- Validate PID control works + +### **2. Performance Test** +- Compare FPS with previous model +- Check detection quality +- Test in different lighting conditions + +### **3. Robustness Test** +- Test with different lane types +- Check behavior in curves +- Validate detection in adverse conditions + +## **Next Steps** + +1. **Run tests in real environment** +2. **Adjust parameters if necessary** +3. **Document observed performance** +4. **Consider future optimizations** + +--- + +**Migration date**: 18/07/2025 +**Status**: Completed and tested + + diff --git a/complete_route/docs/docs_main.md b/complete_route/docs/docs_main.md new file mode 100644 index 000000000..1bca3f8d0 --- /dev/null +++ b/complete_route/docs/docs_main.md @@ -0,0 +1,90 @@ +# `sources/main.cpp` – PID control with joystick & shared‑memory mask + +This file wires the entire **JetRacer autonomous lane‑keeping loop**: + +* **Input** – receives a pre‑segmented **binary lane mask** from another process via POSIX shared memory (`/dev/shm/mask_shared`). +* **Perception** – calls `jetracer::pid::PIDexecute` to extract the lateral error and compute the steering angle. +* **Actuation** – sends the filtered command to the steering servo using `JetRacer::smooth_steering()`. +* **Human interaction** – throttle remains under manual joystick control; only steering is corrected. + +> **Note**: Requires a **producer process** (Python or C++) that writes a 128 × 128 mask into the same shared memory area (byte 0 = flag, bytes 1‑16384 = image). + +--- + +## Execution flow + +```mermaid +graph TD + A[Program start] --> B["Init JetRacer (I2C 0x40 / 0x60)"] + B --> C["Map shared memory \"mask_shared\""] + C --> D[while true] + D -->|flag==0| D + D -->|flag==1| E[PIDexecute with mask] + E --> F["smooth_steering(angle)"] + F --> G[flag = 0] + G --> D +``` + +--- + +## Shared‑memory layout + +| Offset | Size (bytes) | Purpose | +| -----: | -----------: | ------------------------------------------------- | +| `0` | `1` | **flag** – 1 ⇒ new image ready / 0 ⇒ processed | +| `1` | `16384` | **mask** – 128 × 128, 8‑bit grayscale (lane mask) | + +--- + +## Key steps in `main()` + +1. Print banner. +2. Instantiate `JetRacer` with I2C addresses `0x40` (servo) and `0x60` (motor). Store pointer for safe stop. +3. Register `signal_handler()` for **SIGINT**; ensures `stop()` on Ctrl + C. +4. Open and map the POSIX shared memory `mask_shared`. +5. Create a zero‑copy **`cv::Mat mask`** that wraps the mapped region. +6. **Infinite loop**: + + * Wait until `flag == 1` (new mask). + * Generate sequential filename `frame_XXXX.jpg` for overlay. + * Run `PIDexecute(mask.clone(), filename)` – `clone` used because the pipeline draws on the image. + * Call `smooth_steering(angle, 5)` to soften steps. + * Reset `flag` to 0 to signal “mask processed”. + * Break on **Esc** key. +7. Call `stop()`, print “Finishing.”, exit. + +--- + +## Signals & error handling + +* **Ctrl + C** triggers `signal_handler()`, which stops the JetRacer immediately. +* Exceptions are caught; error message printed and `stop()` called. +* Robust checks on `shm_open` / `mmap`; failure → `stderr` + exit code 1. + +--- + +## Build & run + +```bash + +# Terminal 2: compile +make + +# Terminal 1: producer writing masks to shared memory +python3 scripts/camera_yolo_to_shm.py + +# Terminal 2: this C++ binary +./bin//jetracer_pid_controler +``` + +**Dependencies** + +* OpenCV ≥ 4.5 +* JetRacer libraries (`jetracer::control`, `jetracer::vision`, `jetracer::pid`) +* POSIX shared memory, mmap, signals (Linux) + +--- + +## References + +* POSIX Shared Memory (`shm_open`, `mmap`) diff --git a/pid_control/includes/jetracer/computer_vision.hpp b/complete_route/includes/jetracer/computer_vision.hpp similarity index 88% rename from pid_control/includes/jetracer/computer_vision.hpp rename to complete_route/includes/jetracer/computer_vision.hpp index 88ee53ecd..b25ce7764 100644 --- a/pid_control/includes/jetracer/computer_vision.hpp +++ b/complete_route/includes/jetracer/computer_vision.hpp @@ -7,7 +7,6 @@ namespace jetracer::vision { - // Constantes para dimensões da imagem e memória compartilhada constexpr int WIDTH = 128; constexpr int HEIGHT = 128; constexpr int SIZE = WIDTH * HEIGHT; @@ -35,6 +34,6 @@ namespace jetracer::vision float image_center, float center_track, float y_ref); -} // namespace jetracer::vision +} -#endif // COMPUTER_VISION_HPP +#endif diff --git a/complete_route/includes/jetracer/i2c_device.hpp b/complete_route/includes/jetracer/i2c_device.hpp new file mode 100644 index 000000000..3bdbfb085 --- /dev/null +++ b/complete_route/includes/jetracer/i2c_device.hpp @@ -0,0 +1,23 @@ +#ifndef I2C_DEVICE_HPP +#define I2C_DEVICE_HPP + +#include +#include + +namespace jetracer::hardware +{ + class I2CDevice + { + public: + I2CDevice(const std::string &device, int address); + ~I2CDevice(); + + void write_byte(uint8_t reg, uint8_t value); + uint8_t read_byte(uint8_t reg); + + private: + int fd_; + }; +} + +#endif diff --git a/complete_route/includes/jetracer/jetracer.hpp b/complete_route/includes/jetracer/jetracer.hpp new file mode 100644 index 000000000..098de6247 --- /dev/null +++ b/complete_route/includes/jetracer/jetracer.hpp @@ -0,0 +1,89 @@ +#ifndef JETRACER_HPP +#define JETRACER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "jetracer/i2c_device.hpp" +#include "jetracer/pwm_config.hpp" +#include "jetracer/motor_control.hpp" + +namespace jetracer::control +{ + class JetRacer + { + public: + JetRacer(int servo_addr, int motor_addr); + ~JetRacer(); + + void start(); + void stop(); + bool is_running() const; + void set_speed(float speed); + void set_steering(int angle); + void smooth_steering(int target_angle, int increment); + + void set_constant_speed_mode(bool enabled); + void set_test_speed(float speed_percent); + void set_test_duration(int seconds); + void start_speed_test(); + void stop_speed_test(); + bool is_test_mode() const { return test_mode_; } + + int servo_delay_ms_ = 30; + + static constexpr int PWM_FREQUENCY_HZ = pwm::frequency::MOTOR_MED_FREQ; + static constexpr int SPEED_SMOOTHING_WINDOW = pwm::smoothing::BALANCED; + static constexpr float MAX_SPEED_CHANGE_PER_UPDATE = pwm::smoothing::MEDIUM_CHANGE; + + private: + void init_servo(); + void init_motors(); + void set_servo_pwm(int channel, int on_value, int off_value); + void set_motor_pwm(int channel, int value); + void set_motor_pwm_smooth(int channel, int value); + void process_joystick(); + void process_test_mode(); + float smooth_speed(float target_speed); + float calculate_safe_speed(float target_speed); + + static constexpr int MAX_ANGLE_ = 140; + static constexpr int SERVO_LEFT_PWM_ = 140; + static constexpr int SERVO_CENTER_PWM_ = 280; + static constexpr int SERVO_RIGHT_PWM_ = 420; + + int servo_addr_; + int motor_addr_; + std::atomic running_; + hardware::I2CDevice servo_device_; + hardware::I2CDevice motor_device_; + int current_angle_ = 0; + float current_speed_ = 0.0f; + + std::deque speed_history_; + float smoothed_speed_ = 0.0f; + + float filtered_speed_ = 0.0f; + float target_speed_ = 0.0f; + float last_speed_command_ = 0.0f; + unsigned long last_movement_time_ = 0; + + std::atomic test_mode_{false}; + std::atomic test_speed_{0.0f}; + std::atomic test_duration_{0}; + std::atomic test_running_{false}; + std::chrono::steady_clock::time_point test_start_time_; + std::thread test_thread_; + }; +} + +#endif diff --git a/complete_route/includes/jetracer/motor_control.hpp b/complete_route/includes/jetracer/motor_control.hpp new file mode 100644 index 000000000..adc0384da --- /dev/null +++ b/complete_route/includes/jetracer/motor_control.hpp @@ -0,0 +1,44 @@ +#ifndef MOTOR_CONTROL_HPP +#define MOTOR_CONTROL_HPP + +namespace jetracer::motor_control +{ + // Deadzone and threshold settings + namespace thresholds + { + static constexpr float SPEED_DEADZONE = 0.001f; // 0.1% - Minimum deadzone for maximum responsiveness + static constexpr int MIN_PWM_THRESHOLD = 500; // Minimum PWM reduced for slower car (12%) + static constexpr int TORQUE_BOOST_THRESHOLD = 1200; // Threshold reduced for less amplification + static constexpr float LOW_SPEED_AMPLIFICATION = 1.8f; // Reduced amplification for slower car + } + + // Power curve settings + namespace power + { + static constexpr float POWER_CURVE_FACTOR = 0.7f; // Exponential power curve factor (reduced for slower car) + static constexpr float SPEED_SMOOTHING_FACTOR = 0.88f; // Speed smoothing factor (reduced) + } + + // Acceleration ramp settings + namespace ramps + { + static constexpr float ACCELERATION_RAMP = 8.0f; // Greatly increased acceleration rate (% per update) + static constexpr float DECELERATION_RAMP = 15.0f; // Greatly increased deceleration rate (% per update) + static constexpr float EMERGENCY_BRAKE_THRESHOLD = 50.0f; // Threshold for emergency braking + } + + // Safety settings + namespace safety + { + static constexpr float DIRECTION_CHANGE_THRESHOLD = 10.0f; // Threshold to detect direction change + static constexpr float MAX_SPEED_PERCENT = 100.0f; // Maximum allowed speed (no artificial limitation) + + // Speed limitations for safety in turns + static constexpr float BASE_MAX_SPEED = 40.0f; + static constexpr float CURVE_SPEED_REDUCTION = 1.1f; // Speed boost in turns (110% of base speed) - reduced + static constexpr float STRAIGHT_SPEED_BOOST = 1.05f; // No boost on straights (100% of base speed) + static constexpr float STEERING_ANGLE_THRESHOLD = 25.0f; // Steering angle to consider as "turn" - increased to be less sensitive + } +} + +#endif // MOTOR_CONTROL_HPP diff --git a/pid_control/includes/jetracer/pid_controller.hpp b/complete_route/includes/jetracer/pid_controller.hpp similarity index 86% rename from pid_control/includes/jetracer/pid_controller.hpp rename to complete_route/includes/jetracer/pid_controller.hpp index a5fc9e6ab..dc0c6a85a 100644 --- a/pid_control/includes/jetracer/pid_controller.hpp +++ b/complete_route/includes/jetracer/pid_controller.hpp @@ -13,6 +13,6 @@ namespace jetracer::pid }; float PIDapply(float error, float dt, PIDStatus &status); float PIDexecute(const cv::Mat &original_frame, const std::string &base_name); -} // namespace jetracer::pid +} -#endif // PID_CONTROLLER_HPP +#endif diff --git a/complete_route/includes/jetracer/pwm_config.hpp b/complete_route/includes/jetracer/pwm_config.hpp new file mode 100644 index 000000000..71a400287 --- /dev/null +++ b/complete_route/includes/jetracer/pwm_config.hpp @@ -0,0 +1,48 @@ +#ifndef PWM_CONFIG_HPP +#define PWM_CONFIG_HPP + +namespace jetracer::pwm +{ + // PWM frequency settings + namespace frequency + { + static constexpr int MOTOR_LOW_FREQ = 500; + static constexpr int MOTOR_MED_FREQ = 1000; + static constexpr int MOTOR_HIGH_FREQ = 2000; + static constexpr int SERVO_FREQ = 50; + } + + // Smoothing settings + namespace smoothing + { + // Smoothing windows for different types of movement + static constexpr int AGGRESSIVE = 3; + static constexpr int BALANCED = 5; + static constexpr int SMOOTH = 7; + static constexpr int ULTRA_SMOOTH = 10; + + // Speed change limits per update + static constexpr float FAST_CHANGE = 3.0f; + static constexpr float MEDIUM_CHANGE = 1.5f; + static constexpr float SLOW_CHANGE = 0.8f; + static constexpr float ULTRA_SLOW_CHANGE = 0.3f; + } + + // Timing settings + namespace timing + { + // Control loop update frequencies + static constexpr int JOYSTICK_UPDATE_MS = 25; + static constexpr int PID_UPDATE_MS = 30; + static constexpr int MOTOR_UPDATE_MS = 10; + } + + // Safety settings + namespace safety + { + static constexpr float MAX_SPEED_PERCENT = 46.0f; + static constexpr float EMERGENCY_STOP_DELAY_MS = 150.0f; + } +} + +#endif // PWM_CONFIG_HPP diff --git a/complete_route/models/best_202507181755.pt b/complete_route/models/best_202507181755.pt new file mode 100644 index 000000000..66d963056 Binary files /dev/null and b/complete_route/models/best_202507181755.pt differ diff --git a/pid_control/scripts/camera_yolo_to_shm.py b/complete_route/scripts/camera_yolo_to_shm.py similarity index 75% rename from pid_control/scripts/camera_yolo_to_shm.py rename to complete_route/scripts/camera_yolo_to_shm.py index 4b38007b1..60264e626 100644 --- a/pid_control/scripts/camera_yolo_to_shm.py +++ b/complete_route/scripts/camera_yolo_to_shm.py @@ -6,23 +6,23 @@ from ultralytics import YOLO from multiprocessing import shared_memory -# ===== Configurações ===== +# ===== Configuration ===== IMG_WIDTH = 128 IMG_HEIGHT = 128 SHM_NAME = "mask_shared" -MODEL_PATH = "models/best.pt" -LANE_CLASS_ID = 80 +MODEL_PATH = "models/best_202507181755.pt" +LANE_CLASS_ID = 1 CONF_THRESHOLD = 0.25 SAVE_DIR = "masks" -# ===== GStreamer pipeline da câmera CSI ===== +# ===== GStreamer pipeline for CSI camera ===== PIPELINE = ( "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=320, height=240, format=NV12, framerate=15/1 ! " "nvvidconv ! video/x-raw, format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink drop=true max-buffers=1" ) -# ===== Inicializa memória compartilhada (flag + imagem) ===== -TOTAL_SIZE = 1 + IMG_WIDTH * IMG_HEIGHT # 1 byte para flag +# ===== Initialize shared memory (flag + image) ===== +TOTAL_SIZE = 1 + IMG_WIDTH * IMG_HEIGHT # 1 byte for flag try: shm = shared_memory.SharedMemory(name=SHM_NAME, create=True, size=TOTAL_SIZE) except FileExistsError: @@ -34,22 +34,18 @@ flag_buf = np.ndarray((1,), dtype=np.uint8, buffer=shm.buf, offset=0) shm_buf = np.ndarray((IMG_HEIGHT, IMG_WIDTH), dtype=np.uint8, buffer=shm.buf, offset=1) -# ===== Carrega o modelo YOLO e a câmera ===== +# ===== Load YOLO model and camera ===== model = YOLO(MODEL_PATH) cap = cv2.VideoCapture(PIPELINE, cv2.CAP_GSTREAMER) -#for _ in range(5): -# cap.read() -# time.sleep(0.05) - if not cap.isOpened(): - print("Erro ao abrir a câmera CSI.") + print("Error opening CSI camera.") shm.close() shm.unlink() exit(1) os.makedirs(SAVE_DIR, exist_ok=True) -print("Câmera e modelo carregados. Pressione ESC para sair.") +print("Camera and model loaded. Press ESC to exit.") try: while True: @@ -57,7 +53,7 @@ ret, frame = cap.read() if not ret: - print("Frame não capturado.") + print("Frame not captured.") break results = model.predict(source=frame, conf=CONF_THRESHOLD, verbose=False) @@ -74,26 +70,23 @@ mask_i = (mask_i > 0.5).astype(np.uint8) mask_final = np.logical_or(mask_final, mask_i) except Exception as e: - print(f"️Erro ao processar máscara {i}: {e}") + print(f"Error processing mask {i}: {e}") mask_final = (mask_final * 255).astype(np.uint8) mask_resized = cv2.resize(mask_final, (IMG_WIDTH, IMG_HEIGHT)) - # === Sincronização: espera o C++ processar (flag == 0) === + # === Synchronization: wait for C++ to process (flag == 0) === while flag_buf[0] != 0: time.sleep(0.001) - # === Enviar imagem e sinalizar (flag = 1) === + # === Send image and signal (flag = 1) === shm_buf[:] = mask_resized[:] flag_buf[0] = 1 fps = 1 / (time.time() - start) print(f"FPS: {fps:.2f}") - cv2.imshow("Câmera CSI", frame) - cv2.imshow("Máscara Lane", mask_resized) - if cv2.waitKey(1) == 27: break @@ -102,4 +95,4 @@ shm.close() shm.unlink() cv2.destroyAllWindows() - print("Encerrado.") + print("Shutdown.") diff --git a/complete_route/scripts/extract_info_yolo_model.py b/complete_route/scripts/extract_info_yolo_model.py new file mode 100644 index 000000000..30396eb6d --- /dev/null +++ b/complete_route/scripts/extract_info_yolo_model.py @@ -0,0 +1,50 @@ +from ultralytics import YOLO +import torch +import os + +# Model path +model_path = '../models/best_202507181755.pt' + +# Check if file exists +if not os.path.exists(model_path): + print(f"[ERROR] File '{model_path}' not found.") + exit(1) + +print(f"\n[INFO] Loading model: {model_path}") +model = YOLO(model_path) + +# Basic information +print("\nBasic model information:") +print(f"- Model type: {type(model)}") +print(f"- Number of classes: {model.model.nc}") +print(f"- Class names (model.names): {model.names}") + +# Model structure +print("\nModel structure (summary):") +model.info(verbose=True) + +# Training arguments and hyperparameters +print("\nTraining arguments and hyperparameters available in the model:") +try: + args = model.model.args + for k, v in vars(args).items(): + print(f" - {k}: {v}") +except Exception as e: + print(" [!] Could not access 'args' from the Ultralytics model.") + +# Extra: inspect using PyTorch +print("\nDirect inspection using PyTorch:") +try: + raw_model = torch.load(model_path, map_location='cpu') + print("Keys found in the PyTorch dictionary:") + print(list(raw_model.keys())) + + if 'train_args' in raw_model: + print("\nTraining arguments ('train_args') found:") + for k, v in raw_model['train_args'].items(): + print(f" - {k}: {v}") + else: + print(" [!] No 'train_args' found in the model.") + +except Exception as e: + print(f"[ERROR] Failed to load model with PyTorch: {e}") diff --git a/pid_control/sources/computer_vision.cpp b/complete_route/sources/computer_vision.cpp similarity index 99% rename from pid_control/sources/computer_vision.cpp rename to complete_route/sources/computer_vision.cpp index 92a85b0dd..b51eef902 100644 --- a/pid_control/sources/computer_vision.cpp +++ b/complete_route/sources/computer_vision.cpp @@ -113,4 +113,4 @@ namespace jetracer::vision } return -1.0f; } -} // namespace jetracer::vision +} diff --git a/complete_route/sources/i2c_device.cpp b/complete_route/sources/i2c_device.cpp new file mode 100644 index 000000000..8333ac77a --- /dev/null +++ b/complete_route/sources/i2c_device.cpp @@ -0,0 +1,59 @@ +#include "jetracer/i2c_device.hpp" +#include +#include +#include +#include +#include + +namespace jetracer::hardware +{ + + I2CDevice::I2CDevice(const std::string &device, int address) + { + fd_ = open(device.c_str(), O_RDWR); + if (fd_ < 0) + { + throw std::runtime_error("Failed to open I2C device: " + device); + } + if (ioctl(fd_, I2C_SLAVE, address) < 0) + { + close(fd_); + throw std::runtime_error("Failed to set I2C address"); + } + } + + I2CDevice::~I2CDevice() + { + if (fd_ >= 0) + { + close(fd_); + } + } + + void I2CDevice::write_byte(uint8_t reg, uint8_t value) + { + uint8_t buffer[2] = {reg, value}; + if (write(fd_, buffer, 2) != 2) + { + close(fd_); + throw std::runtime_error("Failed to write to I2C device"); + } + } + + uint8_t I2CDevice::read_byte(uint8_t reg) + { + if (write(fd_, ®, 1) != 1) + { + close(fd_); + throw std::runtime_error("Failed to write register to I2C device"); + } + uint8_t value; + if (read(fd_, &value, 1) != 1) + { + close(fd_); + throw std::runtime_error("Failed to read from I2C device"); + } + return value; + } + +} diff --git a/complete_route/sources/jetracer.cpp b/complete_route/sources/jetracer.cpp new file mode 100644 index 000000000..137db7614 --- /dev/null +++ b/complete_route/sources/jetracer.cpp @@ -0,0 +1,680 @@ +#include "jetracer/jetracer.hpp" +#include +#include +#include +#include + +namespace jetracer::control +{ + JetRacer::JetRacer(int servo_addr, int motor_addr) + : servo_addr_(servo_addr), + motor_addr_(motor_addr), + running_(false), + servo_device_("/dev/i2c-1", servo_addr), + motor_device_("/dev/i2c-1", motor_addr) + { + init_servo(); + init_motors(); + } + + JetRacer::~JetRacer() + { + stop(); + } + + void JetRacer::init_servo() + { + try + { + servo_device_.write_byte(0x00, 0x06); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0x00, 0x10); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0xFE, 0x79); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0x01, 0x04); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0x00, 0x20); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + catch (const std::exception &e) + { + std::cerr << "Servo initialization failed: " << e.what() << std::endl; + stop(); + } + } + + void JetRacer::init_motors() + { + try + { + motor_device_.write_byte(0x00, 0x20); + + int prescale = static_cast(std::floor(25000000.0 / 4096.0 / PWM_FREQUENCY_HZ - 1)); + int oldmode = motor_device_.read_byte(0x00); + int newmode = (oldmode & 0x7F) | 0x10; + + motor_device_.write_byte(0x00, newmode); + motor_device_.write_byte(0xFE, prescale); + motor_device_.write_byte(0x00, oldmode); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + motor_device_.write_byte(0x00, oldmode | 0xA1); + + std::cout << "[INFO] Motors initialized with PWM frequency: " << PWM_FREQUENCY_HZ << " Hz" << std::endl; + std::cout << "[INFO] Calculated prescale: " << prescale << std::endl; + } + catch (const std::exception &e) + { + std::cerr << "Motor initialization failed: " << e.what() << std::endl; + stop(); + } + } + + void JetRacer::set_steering(int angle) + { + angle = std::clamp(angle, -MAX_ANGLE_, MAX_ANGLE_); + + int pwm = 0; + if (angle < 0) + { + std::cout << "Setting steering to left: " << angle << std::endl; + pwm = SERVO_CENTER_PWM_ + (angle / static_cast(MAX_ANGLE_)) * (SERVO_CENTER_PWM_ - SERVO_LEFT_PWM_); + } + else if (angle > 0) + { + pwm = SERVO_CENTER_PWM_ + (angle / static_cast(MAX_ANGLE_)) * (SERVO_RIGHT_PWM_ - SERVO_CENTER_PWM_); + std::cout << "Setting steering to right: " << angle << std::endl; + } + else + { + pwm = SERVO_CENTER_PWM_; + std::cout << "Setting steering to center: " << angle << std::endl; + } + + set_servo_pwm(0, 0, pwm); + current_angle_ = angle; + + std::this_thread::sleep_for(std::chrono::milliseconds(servo_delay_ms_)); + } + + void JetRacer::smooth_steering(int target_angle, int increment) + { + target_angle = std::clamp(target_angle, -MAX_ANGLE_, MAX_ANGLE_); + int step = (target_angle > current_angle_) ? increment : -increment; + + while ((step > 0 && current_angle_ < target_angle) || (step < 0 && current_angle_ > target_angle)) + { + current_angle_ += step; + if ((step > 0 && current_angle_ > target_angle) || (step < 0 && current_angle_ < target_angle)) + { + current_angle_ = target_angle; + } + set_steering(current_angle_); + } + } + + void JetRacer::set_servo_pwm(int channel, int on_value, int off_value) + { + int base_reg = 0x06 + (channel * 4); + servo_device_.write_byte(base_reg, on_value & 0xFF); + servo_device_.write_byte(base_reg + 1, on_value >> 8); + servo_device_.write_byte(base_reg + 2, off_value & 0xFF); + servo_device_.write_byte(base_reg + 3, off_value >> 8); + } + + void JetRacer::set_motor_pwm(int channel, int value) + { + value = std::clamp(value, 0, 4095); + int base_reg = 0x06 + (channel * 4); + motor_device_.write_byte(base_reg, 0); + motor_device_.write_byte(base_reg + 1, 0); + motor_device_.write_byte(base_reg + 2, value & 0xFF); + motor_device_.write_byte(base_reg + 3, value >> 8); + } + + float JetRacer::smooth_speed(float target_speed) + { + + speed_history_.push_back(target_speed); + + if (speed_history_.size() > SPEED_SMOOTHING_WINDOW) + { + speed_history_.pop_front(); + } + + float sum = 0.0f; + for (float speed : speed_history_) + { + sum += speed; + } + float average_speed = sum / speed_history_.size(); + + float max_change = MAX_SPEED_CHANGE_PER_UPDATE; + float speed_diff = average_speed - smoothed_speed_; + + if (std::abs(speed_diff) > max_change) + { + if (speed_diff > 0) + { + smoothed_speed_ += max_change; + } + else + { + smoothed_speed_ -= max_change; + } + } + else + { + smoothed_speed_ = average_speed; + } + + return smoothed_speed_; + } + + float JetRacer::calculate_safe_speed(float target_speed) + { + + float max_safe_speed = motor_control::safety::BASE_MAX_SPEED; + + bool is_turning = std::abs(current_angle_) > motor_control::safety::STEERING_ANGLE_THRESHOLD; + + bool is_car_stopped = (std::abs(current_speed_) < 1.0f); + + if (is_turning && !is_car_stopped) + { + + max_safe_speed *= motor_control::safety::CURVE_SPEED_REDUCTION; + + static int curve_debug_counter = 0; + if ((++curve_debug_counter % 50) == 0) + { + std::cout << "[CURVE] Angle: " << current_angle_ + << "°, Max speed: " << max_safe_speed << "% (boost applied to overcome resistance)" << std::endl; + } + } + else if (!is_turning) + { + + max_safe_speed *= motor_control::safety::STRAIGHT_SPEED_BOOST; + + static int straight_debug_counter = 0; + if ((++straight_debug_counter % 100) == 0) + { + std::cout << "[STRAIGHT] Angle: " << current_angle_ + << "°, Max speed: " << max_safe_speed << "% (normal speed)" << std::endl; + } + } + else + { + + if (is_car_stopped && is_turning) + { + static int startup_curve_debug_counter = 0; + if ((++startup_curve_debug_counter % 30) == 0) + { + std::cout << "[STARTUP_CURVE] Car stopped in turn - allowing maximum speed to overcome inertia of turned wheels" << std::endl; + } + } + } + + if (std::abs(target_speed) > max_safe_speed) + { + + float sign = (target_speed > 0) ? 1.0f : -1.0f; + target_speed = sign * max_safe_speed; + + static int limit_debug_counter = 0; + if ((++limit_debug_counter % 30) == 0) + { + std::cout << "[SAFETY] Speed limited to " << target_speed + << "% (max safe: " << max_safe_speed << "%)" << std::endl; + } + } + + return target_speed; + } + + void JetRacer::set_motor_pwm_smooth(int channel, int value) + { + + value = std::clamp(value, 0, 4095); + int base_reg = 0x06 + (channel * 4); + + bool is_car_stopped = (std::abs(current_speed_) < 1.0f); + + if (is_car_stopped) + { + + motor_device_.write_byte(base_reg, 0); + motor_device_.write_byte(base_reg + 1, 0); + motor_device_.write_byte(base_reg + 2, value & 0xFF); + motor_device_.write_byte(base_reg + 3, value >> 8); + return; + } + + static std::array last_pwm_values = {0}; + static std::array target_pwm_values = {0}; + + target_pwm_values[channel] = value; + + int current_pwm = last_pwm_values[channel]; + int pwm_diff = target_pwm_values[channel] - current_pwm; + + int max_pwm_change = 200; + if (std::abs(pwm_diff) > max_pwm_change) + { + if (pwm_diff > 0) + { + current_pwm += max_pwm_change; + } + else + { + current_pwm -= max_pwm_change; + } + } + else + { + current_pwm = target_pwm_values[channel]; + } + + last_pwm_values[channel] = current_pwm; + + motor_device_.write_byte(base_reg, 0); + motor_device_.write_byte(base_reg + 1, 0); + motor_device_.write_byte(base_reg + 2, current_pwm & 0xFF); + motor_device_.write_byte(base_reg + 3, current_pwm >> 8); + } + + void JetRacer::set_speed(float speed) + { + + static int set_speed_debug_counter = 0; + + if ((++set_speed_debug_counter % 20) == 0) + { + std::cout << "[DEBUG] set_speed() - Requested speed: " << speed << "%" << std::endl; + } + + float original_speed = speed; + speed = calculate_safe_speed(speed); + + unsigned long current_time = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + + if (std::abs(original_speed) != std::abs(speed) && set_speed_debug_counter % 20 == 0) + { + std::cout << "[SAFETY] Speed adjusted from " << original_speed + << "% to " << speed << "% for safety" << std::endl; + } + + bool emergency_brake = false; + float speed_reduction = std::abs(speed) - std::abs(last_speed_command_); + + if (last_speed_command_ != 0.0f && speed_reduction < -motor_control::ramps::EMERGENCY_BRAKE_THRESHOLD) + { + emergency_brake = true; + } + + if ((last_speed_command_ > motor_control::safety::DIRECTION_CHANGE_THRESHOLD && speed < -motor_control::safety::DIRECTION_CHANGE_THRESHOLD) || + (last_speed_command_ < -motor_control::safety::DIRECTION_CHANGE_THRESHOLD && speed > motor_control::safety::DIRECTION_CHANGE_THRESHOLD)) + { + emergency_brake = true; + } + + if (std::abs(speed) < motor_control::thresholds::SPEED_DEADZONE * 100.0f) + { + speed = 0.0f; + } + + bool is_car_stopped = (std::abs(current_speed_) < 1.0f); + + if (is_car_stopped && std::abs(speed) > 5.0f && set_speed_debug_counter % 10 == 0) + { + std::cout << "[STARTUP] Car stopped - applying direct acceleration: " << speed << "%" << std::endl; + } + + float power_curve = 1.0f; + if (std::abs(speed) > 0.0f) + { + + power_curve = 1.0f + (std::abs(speed) / 100.0f) * motor_control::power::POWER_CURVE_FACTOR; + speed *= power_curve; + } + + speed = std::max(-100.0f, std::min(speed, 100.0f)); + + if (emergency_brake) + { + + filtered_speed_ = speed; + target_speed_ = speed; + } + else if (is_car_stopped) + { + + filtered_speed_ = speed; + target_speed_ = speed; + if (set_speed_debug_counter % 10 == 0) + { + std::cout << "[STARTUP] Car really stopped - applying direct acceleration: " << speed << "%" << std::endl; + } + } + else if (std::abs(speed - current_speed_) > 5.0f) + { + + filtered_speed_ = speed; + target_speed_ = speed; + if (set_speed_debug_counter % 10 == 0) + { + std::cout << "[RESPONSIVE] Sudden change detected - applying direct command: " << speed << "%" << std::endl; + } + } + else + { + + filtered_speed_ = motor_control::power::SPEED_SMOOTHING_FACTOR * speed + + (1.f - motor_control::power::SPEED_SMOOTHING_FACTOR) * filtered_speed_; + } + + if (!emergency_brake && !is_car_stopped && std::abs(speed - current_speed_) <= 5.0f) + { + + float speed_diff = filtered_speed_ - target_speed_; + float ramp_rate; + + bool is_braking = (filtered_speed_ < target_speed_ && target_speed_ > 0) || + (filtered_speed_ > target_speed_ && target_speed_ < 0) || + (std::abs(filtered_speed_) < std::abs(target_speed_)); + + if (is_braking) + { + + ramp_rate = motor_control::ramps::DECELERATION_RAMP; + } + else + { + + ramp_rate = motor_control::ramps::ACCELERATION_RAMP; + } + + if (std::abs(speed_diff) > ramp_rate) + { + target_speed_ += (speed_diff > 0 ? ramp_rate : -ramp_rate); + } + else + { + target_speed_ = filtered_speed_; + } + } + else + { + + target_speed_ = filtered_speed_; + if (set_speed_debug_counter % 10 == 0) + { + std::cout << "[DIRECT] Applying direct command (no ramp): " << filtered_speed_ << "%" << std::endl; + } + } + + int pwm_value = static_cast(std::abs(target_speed_) / 100.0f * 4095); + + if (set_speed_debug_counter % 20 == 0) + { + std::cout << "[DEBUG] set_speed() - Calculated PWM: " << pwm_value + << " for speed: " << target_speed_ << "%" << std::endl; + } + + if (pwm_value > 0 && pwm_value < motor_control::thresholds::MIN_PWM_THRESHOLD) + { + pwm_value = motor_control::thresholds::MIN_PWM_THRESHOLD; + } + + if (pwm_value > 0 && pwm_value < 1000) + { + pwm_value = static_cast(pwm_value * 2.0f); + } + + if (pwm_value > 0 && pwm_value < motor_control::thresholds::TORQUE_BOOST_THRESHOLD) + { + pwm_value = static_cast(pwm_value * motor_control::thresholds::LOW_SPEED_AMPLIFICATION); + } + + if (is_car_stopped && pwm_value > 0) + { + + float boost_multiplier = 2.0f; + + if (std::abs(current_angle_) > motor_control::safety::STEERING_ANGLE_THRESHOLD) + { + boost_multiplier = 3.5f; + static int curve_startup_debug_counter = 0; + if ((++curve_startup_debug_counter % 10) == 0) + { + std::cout << "[STARTUP_CURVE] Applying extra boost for car stopped in turn: PWM " << pwm_value << std::endl; + } + } + + pwm_value = static_cast(pwm_value * boost_multiplier); + + static int startup_debug_counter = 0; + if ((++startup_debug_counter % 10) == 0) + { + std::cout << "[STARTUP] Applying startup boost for car really stopped: PWM " << pwm_value << std::endl; + } + } + + pwm_value = std::min(pwm_value, 4095); + + if (set_speed_debug_counter % 20 == 0) + { + std::cout << "[DEBUG] set_speed() - Target: " << target_speed_ + << "%, PWM: " << pwm_value + << ", Filtered: " << filtered_speed_ << "%" << std::endl; + } + + if (target_speed_ > 0) + { + set_motor_pwm_smooth(0, pwm_value); + set_motor_pwm_smooth(1, 0); + set_motor_pwm_smooth(2, pwm_value); + set_motor_pwm_smooth(5, pwm_value); + set_motor_pwm_smooth(6, 0); + set_motor_pwm_smooth(7, pwm_value); + } + else if (target_speed_ < 0) + { + set_motor_pwm_smooth(0, pwm_value); + set_motor_pwm_smooth(1, pwm_value); + set_motor_pwm_smooth(2, 0); + set_motor_pwm_smooth(6, pwm_value); + set_motor_pwm_smooth(7, pwm_value); + set_motor_pwm_smooth(8, 0); + } + else + { + + for (int channel = 0; channel < 9; ++channel) + { + set_motor_pwm_smooth(channel, 0); + } + } + + current_speed_ = target_speed_; + last_speed_command_ = speed; + + if (std::abs(speed) > 1.0f) + { + last_movement_time_ = current_time; + } + } + + void JetRacer::set_constant_speed_mode(bool enabled) + { + test_mode_ = enabled; + if (enabled) + { + std::cout << "[TEST MODE] Constant speed test mode ACTIVATED" << std::endl; + std::cout << "[TEST MODE] Use set_test_speed() and start_speed_test() to test" << std::endl; + } + else + { + std::cout << "[TEST MODE] Test mode DEACTIVATED - returning to joystick control" << std::endl; + stop_speed_test(); + } + } + + void JetRacer::set_test_speed(float speed_percent) + { + + speed_percent = std::max(-50.0f, std::min(50.0f, speed_percent)); + test_speed_ = speed_percent; + std::cout << "[TEST MODE] Test speed set to: " << speed_percent << "%" << std::endl; + } + + void JetRacer::set_test_duration(int seconds) + { + test_duration_ = std::max(1, std::min(300, seconds)); + std::cout << "[TEST MODE] Test duration set to: " << test_duration_ << " seconds" << std::endl; + } + + void JetRacer::start_speed_test() + { + if (!test_mode_) + { + std::cout << "[ERROR] Test mode is not activated. Use set_constant_speed_mode(true) first." << std::endl; + return; + } + + if (test_running_) + { + std::cout << "[WARNING] Test already running. Stopping previous test..." << std::endl; + stop_speed_test(); + } + + test_running_ = true; + test_start_time_ = std::chrono::steady_clock::now(); + + test_thread_ = std::thread(&JetRacer::process_test_mode, this); + test_thread_.detach(); + + std::cout << "[TEST MODE] Test started with speed: " << test_speed_ << "% for " << test_duration_ << " seconds" << std::endl; + std::cout << "[TEST MODE] Use stop_speed_test() to stop the test" << std::endl; + } + + void JetRacer::stop_speed_test() + { + if (test_running_) + { + test_running_ = false; + set_speed(0); + std::cout << "[TEST MODE] Test stopped. Car stopped." << std::endl; + } + } + + void JetRacer::process_test_mode() + { + std::cout << "[TEST MODE] Applying constant speed: " << test_speed_ << "%" << std::endl; + + set_speed(test_speed_); + + auto start_time = std::chrono::steady_clock::now(); + while (test_running_) + { + auto current_time = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(current_time - start_time).count(); + + if (elapsed >= test_duration_) + { + std::cout << "[TEST MODE] Test duration reached (" << test_duration_ << "s). Stopping..." << std::endl; + break; + } + + set_speed(test_speed_); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + if (test_running_) + { + set_speed(0); + test_running_ = false; + std::cout << "[TEST MODE] Test finished. Car stopped." << std::endl; + } + } + + void JetRacer::process_joystick() + { + if (SDL_Init(SDL_INIT_JOYSTICK) < 0) + { + std::cerr << "Failed to initialize SDL: " << SDL_GetError() << std::endl; + return; + } + + SDL_Joystick *joystick = SDL_JoystickOpen(0); + if (!joystick) + { + std::cerr << "Failed to open joystick: " << SDL_GetError() << std::endl; + SDL_Quit(); + return; + } + + while (running_) + { + + if (test_mode_ && test_running_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + + static int debug_counter = 0; + if ((++debug_counter % 100) == 0) + { + std::cout << "[JOYSTICK] Processing joystick input..." << std::endl; + } + + SDL_JoystickUpdate(); + + int left_joystick_y = SDL_JoystickGetAxis(joystick, 1); + + if (debug_counter % 100 == 0) + { + float speed_percent = -left_joystick_y / 32767.0f * 100; + std::cout << "[JOYSTICK] Y: " << left_joystick_y << " -> Speed: " << speed_percent << "%" << std::endl; + } + + set_speed(-left_joystick_y / 32767.0f * 100); + + std::this_thread::sleep_for(std::chrono::milliseconds(pwm::timing::JOYSTICK_UPDATE_MS)); + } + + SDL_JoystickClose(joystick); + SDL_Quit(); + } + + void JetRacer::start() + { + running_ = true; + std::thread joystick_thread(&JetRacer::process_joystick, this); + joystick_thread.detach(); + } + + void JetRacer::stop() + { + running_ = false; + stop_speed_test(); + set_speed(0); + set_steering(0); + } + + bool JetRacer::is_running() const + { + return running_.load(); + } + +} diff --git a/pid_control/sources/pid_controller.cpp b/complete_route/sources/pid_controller.cpp similarity index 86% rename from pid_control/sources/pid_controller.cpp rename to complete_route/sources/pid_controller.cpp index a90575444..bc53a9b27 100644 --- a/pid_control/sources/pid_controller.cpp +++ b/complete_route/sources/pid_controller.cpp @@ -11,7 +11,7 @@ namespace jetracer::pid constexpr float Ki = 0.1f; constexpr float Kd = 0.2f; constexpr float MAX_ANGLE = 140.0f; - constexpr float displacement_cm = 21.0f; + constexpr float displacement_cm = 17.0f; float PIDapply(float error, float dt, PIDStatus &status) { @@ -25,6 +25,7 @@ namespace jetracer::pid float PIDexecute(const cv::Mat &original_frame, const std::string &base_name) { + (void)base_name; if (original_frame.empty() || original_frame.channels() != 1) { std::cerr << "Invalid input image." << std::endl; @@ -66,12 +67,6 @@ namespace jetracer::pid std::cout << "Lateral error: " << error << " degrees" << std::endl; std::cout << "PID correction: " << pid_angle << " degrees" << std::endl; - jetracer::vision::draw_overlay(frame, error, pid_angle, base_name, status, image_center, center_track, y_ref); - - fs::create_directories("outputs"); - cv::imwrite("outputs/" + base_name, frame); - cv::imshow("Image with PID", frame); - return pid_angle; } -} // namespace jetracer::pid +} diff --git a/emergency_stop/Makefile b/emergency_stop/Makefile new file mode 100644 index 000000000..c374a1743 --- /dev/null +++ b/emergency_stop/Makefile @@ -0,0 +1,56 @@ +# Compiler and flags +CC = g++ +CFLAGS = -Wall -Wextra -Werror -std=c++17 -O3 -g -Iincludes `pkg-config --cflags opencv4` -MMD +LDFLAGS = -lSDL2 -li2c -lpthread `pkg-config --libs opencv4` -lstdc++fs -lrt -lmosquitto + +# Directories +SRCDIR = sources +APPDIR = apps +OBJDIR = build +BINDIR = bin + +# Sources by module +SRC_TEST_PID = $(APPDIR)/main.cpp $(SRCDIR)/pid_controller.cpp $(SRCDIR)/jetracer.cpp $(SRCDIR)/i2c_device.cpp $(SRCDIR)/computer_vision.cpp + +# Generated objects +OBJ_TEST_PID = $(patsubst %.cpp,$(OBJDIR)/%.o,$(notdir $(SRC_TEST_PID))) + +# Executables +EXEC_TEST_PID = $(BINDIR)/jetracer_pid_controler + +# Main target +all: $(EXEC_TEST_PID) + +# Rules for executables +$(EXEC_TEST_PID): $(OBJ_TEST_PID) | $(BINDIR) + #$(CC) $(CFLAGS) -o $@ $(addprefix $(OBJDIR)/,$(notdir $^)) -lSDL2 -lpthread `pkg-config --libs opencv4` -lstdc++fs + $(CC) $(CFLAGS) -o $@ $(addprefix $(OBJDIR)/,$(notdir $^)) $(LDFLAGS) + +# Rule for object files +$(OBJDIR)/%.o: $(SRCDIR)/%.cpp | $(OBJDIR) + $(CC) $(CFLAGS) -c $< -o $@ + +$(OBJDIR)/%.o: $(APPDIR)/%.cpp | $(OBJDIR) + $(CC) $(CFLAGS) -c $< -o $@ + +# Create build/ directory if necessary +$(OBJDIR): + mkdir -p $(OBJDIR) + +# Create bin/ directory if necessary +$(BINDIR): + mkdir -p $(BINDIR) + +# Auxiliary commands +clean: + rm -rf $(OBJDIR) $(BINDIR) + +re: clean all + +# Targets to run specific ones +testpid: $(EXEC_TEST_PID) + +.PHONY: all clean re control video frames lane frameslane oneframe testpid run + +# Include automatically generated dependencies +-include $(OBJDIR)/*.d diff --git a/emergency_stop/README.md b/emergency_stop/README.md new file mode 100644 index 000000000..c9e1f9ba4 --- /dev/null +++ b/emergency_stop/README.md @@ -0,0 +1,74 @@ +# PID Control + JetRacer Vision + +C++ implementation of lane detection, PID control, and hardware interface for the JetRacer car, integrated with a Python pipeline that writes camera segmentation masks to shared memory. + +> **Requirements** +> • CMake ≥ 3.18 • OpenCV ≥ 4.5 • SDL2 • Linux I2C (i2c-dev) • Python ≥ 3.9 (for camera_yolo_to_shm.py) + +--- + +## Repository Structure + +apps/ # example executables + └─ main.cpp # PID with joystick + shared memory +includes/jetracer/ # public headers (*.hpp) +sources/ # C++ implementations (*.cpp) +models/ # YOLO / LaneNet model (.pt) +scripts/ # Python utilities +docs/ # Markdown documentation + diagrams +Makefile # default target: make && make run + +--- + +## Execution + +```bash +# Terminal 1: producer writes lane masks to shared memory +python3 scripts/camera_yolo_to_shm.py + +# Terminal 2: run the C++ controller +./bin/jetracer_pid_controler +``` + +## Testing + +```bash +# Compile and test manually +make clean && make +./bin/jetracer_pid_controler +``` + +--- + +## Execution Flow (Simplified) + +```mermaid +graph TD + P1["Python – YOLO mask"] --> M1["/dev/shm/mask_shared"] + M1 --> C1["apps/main.cpp"] + C1 --> V1["computer_vision"] + V1 --> PID1["pid_controller"] + PID1 --> J1["JetRacer::smooth_steering"] +``` + +--- + +## PWM Improvements + +The system now includes advanced PWM improvements to eliminate motor speed pulsation: + +- **Higher PWM Frequency**: Increased from 100Hz to 1000Hz (configurable up to 2000Hz) +- **Speed Smoothing**: Moving average filter with configurable smoothing window +- **Rate Limiting**: Maximum speed change per update to prevent sudden movements +- **Configurable Parameters**: Easy adjustment for different use cases + +## Motor Control Improvements + +Advanced motor control system to eliminate "force to start" sound and improve acceleration: + +- **Intelligent Power Curve**: Exponential power amplification for better low-speed response +- **Minimum PWM Threshold**: Guaranteed initial torque to eliminate startup resistance +- **Torque Amplification**: Boost for low-speed PWM values +- **Smart Acceleration Ramp**: Different rates for acceleration vs. deceleration +- **Emergency Braking**: Intelligent detection and response to sudden changes +- **Deadzone Control**: Eliminates oscillations at very low speeds diff --git a/emergency_stop/apps/main.cpp b/emergency_stop/apps/main.cpp new file mode 100644 index 000000000..f086f498b --- /dev/null +++ b/emergency_stop/apps/main.cpp @@ -0,0 +1,1067 @@ +// File: sources/main.cpp +#include "jetracer/pid_controller.hpp" +#include "jetracer/jetracer.hpp" +#include "jetracer/computer_vision.hpp" +#include "jetracer/pid_controller.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ====== MQTT CONFIGURATIONS ====== +// HiveMQ Cloud (ACTIVE) +#define MQTT_BROKER "972e24210b544ba49bfb9c1d3164d02b.s1.eu.hivemq.cloud" +#define MQTT_PORT 8883 +#define MQTT_USERNAME "jetracer" +#define MQTT_PASSWORD "Ft_seame5" + +#define MQTT_TOPIC_LANE_TOUCH "jetracer/lane_touch" +#define MQTT_TOPIC_PASSADEIRA "jetracer/passadeira" +#define MQTT_TOPIC_STOP_SIGN "jetracer/stop_sign" +#define MQTT_TOPIC_SPEED_50 "jetracer/speed_50" +#define MQTT_TOPIC_SPEED_80 "jetracer/speed_80" +#define MQTT_TOPIC_JETRACER "jetracer/jetracer" +#define MQTT_TOPIC_GATE "jetracer/gate" + +// ====== STREAMING CONFIGURATIONS ====== +#define STREAM_IP "100.124.102.80" // Target PC IP +#define STREAM_PORT 5000 // Port for mask streaming +#define STREAM_WIDTH 640 // Stream width +#define STREAM_HEIGHT 480 // Stream height +#define STREAM_FPS 30 // Stream FPS + +// ====== EMERGENCY STOP CONFIGURATIONS ====== +#define EMERGENCY_THRESHOLD 0.15f // 15% danger zone occupancy +#define EMERGENCY_RECOVERY_DELAY_MS 2000 // 2000ms (2 seconds) pause after stop + +// ====== JETRACER VARIABLES ====== +jetracer::control::JetRacer *jetracer_ptr = nullptr; +std::atomic program_running{true}; + +// ====== MQTT VARIABLES ====== +struct mosquitto *mosq = nullptr; +bool mqtt_connected = false; +std::atomic mqtt_running{false}; + +// ====== STREAMING VARIABLES ====== +cv::VideoWriter stream_writer; +bool streaming_initialized = false; + +// ====== EMERGENCY STOP VARIABLES ====== +bool emergency_stop_active = false; +std::chrono::steady_clock::time_point emergency_stop_time; +float speed_before_emergency = 0.0f; +bool cruise_control_before_emergency = false; + +// ====== MQTT FUNCTIONS FOR SPECIFIC CLASSES ====== +void publishPassadeira(bool detected) +{ + if (!mqtt_connected || !mosq) + { + std::cerr << "[MQTT] Not connected, skipping passadeira publication" << std::endl; + return; + } + + std::string message = detected ? "1" : "0"; + int ret = mosquitto_publish(mosq, NULL, MQTT_TOPIC_PASSADEIRA, message.size(), message.c_str(), 0, false); + if (ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[MQTT] Failed to publish passadeira: " << mosquitto_strerror(ret) << std::endl; + mqtt_connected = false; // Mark as disconnected to try reconnecting + } + else + { + // std::cout << "[MQTT] Passadeira: " << message << std::endl; + } +} + +void publishStopSign(bool detected) +{ + if (!mqtt_connected || !mosq) + { + std::cerr << "[MQTT] Não conectado, pulando publicação de stop sign" << std::endl; + return; + } + + std::string message = detected ? "1" : "0"; + int ret = mosquitto_publish(mosq, NULL, MQTT_TOPIC_STOP_SIGN, message.size(), message.c_str(), 0, false); + if (ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[MQTT] Falha ao publicar stop sign: " << mosquitto_strerror(ret) << std::endl; + mqtt_connected = false; // Marcar como desconectado para tentar reconectar + } + else + { + // std::cout << "[MQTT] Stop Sign: " << message << std::endl; + } +} + +void publishSpeed50(bool detected) +{ + if (!mqtt_connected || !mosq) + { + std::cerr << "[MQTT] Não conectado, pulando publicação de speed 50" << std::endl; + return; + } + + std::string message = detected ? "1" : "0"; + int ret = mosquitto_publish(mosq, NULL, MQTT_TOPIC_SPEED_50, message.size(), message.c_str(), 0, false); + if (ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[MQTT] Falha ao publicar speed 50: " << mosquitto_strerror(ret) << std::endl; + mqtt_connected = false; // Marcar como desconectado para tentar reconectar + } + else + { + // std::cout << "[MQTT] Speed 50: " << message << std::endl; + } +} + +void publishSpeed80(bool detected) +{ + if (!mqtt_connected || !mosq) + { + std::cerr << "[MQTT] Não conectado, pulando publicação de speed 80" << std::endl; + return; + } + + std::string message = detected ? "1" : "0"; + int ret = mosquitto_publish(mosq, NULL, MQTT_TOPIC_SPEED_80, message.size(), message.c_str(), 0, false); + if (ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[MQTT] Falha ao publicar speed 80: " << mosquitto_strerror(ret) << std::endl; + mqtt_connected = false; // Marcar como desconectado para tentar reconectar + } + else + { + // std::cout << "[MQTT] Speed 80: " << message << std::endl; + } +} + +void publishJetRacer(bool detected) +{ + if (!mqtt_connected || !mosq) + { + std::cerr << "[MQTT] Não conectado, pulando publicação de jetracer" << std::endl; + return; + } + + std::string message = detected ? "1" : "0"; + int ret = mosquitto_publish(mosq, NULL, MQTT_TOPIC_JETRACER, message.size(), message.c_str(), 0, false); + if (ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[MQTT] Falha ao publicar jetracer: " << mosquitto_strerror(ret) << std::endl; + mqtt_connected = false; // Marcar como desconectado para tentar reconectar + } + else + { + // std::cout << "[MQTT] JetRacer: " << message << std::endl; + } +} + +void publishGate(bool detected) +{ + if (!mqtt_connected || !mosq) + { + std::cerr << "[MQTT] Não conectado, pulando publicação de gate" << std::endl; + return; + } + + std::string message = detected ? "1" : "0"; + int ret = mosquitto_publish(mosq, NULL, MQTT_TOPIC_GATE, message.size(), message.c_str(), 0, false); + if (ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[MQTT] Falha ao publicar gate: " << mosquitto_strerror(ret) << std::endl; + mqtt_connected = false; // Marcar como desconectado para tentar reconectar + } + else + { + // std::cout << "[MQTT] Gate: " << message << std::endl; + } +} + +// ====== CALLBACKS MQTT ====== +void on_connect(struct mosquitto *mosq, void *obj, int rc) +{ + (void)mosq; // Avoid unused parameter warning + (void)obj; // Avoid unused parameter warning + if (rc == 0) + { + // std::cout << "[MQTT] Conectado com sucesso ao broker!" << std::endl; + // std::cout << "[MQTT] Conexão TLS estabelecida e autenticada" << std::endl; + mqtt_connected = true; + } + else + { + std::cerr << "[MQTT] Falha na conexão, código: " << rc << std::endl; + + // Interpretar códigos de falha de conexão + switch (rc) + { + case 1: + std::cerr << "[MQTT] Protocolo incorreto" << std::endl; + break; + case 2: + std::cerr << "[MQTT] Identificador de cliente inválido" << std::endl; + break; + case 3: + std::cerr << "[MQTT] Servidor indisponível" << std::endl; + break; + case 4: + std::cerr << "[MQTT] Credenciais inválidas" << std::endl; + break; + case 5: + std::cerr << "[MQTT] Não autorizado" << std::endl; + break; + case 6: + std::cerr << "[MQTT] Erro de rede" << std::endl; + break; + default: + std::cerr << "[MQTT] Código de erro desconhecido" << std::endl; + break; + } + + mqtt_connected = false; + } +} + +// ====== CALLBACKS MQTT ====== +void on_disconnect(struct mosquitto *mosq, void *obj, int rc) +{ + (void)mosq; // Avoid unused parameter warning + (void)obj; // Avoid unused parameter warning + // std::cout << "[MQTT] Desconectado do broker, código: " << rc << std::endl; + + // Interpretar códigos de desconexão + switch (rc) + { + case 0: + // std::cout << "[MQTT] Desconexão solicitada pelo cliente" << std::endl; + break; + case 1: + // std::cout << "[MQTT] Erro de protocolo incorreto" << std::endl; + break; + case 2: + // std::cout << "[MQTT] Identificador de cliente inválido" << std::endl; + break; + case 3: + // std::cout << "[MQTT] Servidor indisponível" << std::endl; + break; + case 4: + // std::cout << "[MQTT] Credenciais inválidas" << std::endl; + break; + case 5: + // std::cout << "[MQTT] Não autorizado" << std::endl; + break; + case 6: + // std::cout << "[MQTT] Erro de rede" << std::endl; + break; + case 7: + // std::cout << "[MQTT] Conexão perdida (timeout/erro TLS)" << std::endl; + break; + default: + // std::cout << "[MQTT] Código de erro desconhecido" << std::endl; + break; + } + + mqtt_connected = false; +} + +// ====== MQTT INITIALIZATION AND CLEANUP FUNCTIONS ====== +void initMQTT() +{ + mosquitto_lib_init(); + + // Unique ClientID based on PID + std::string cid = "yolov8_detector_" + std::to_string(getpid()); + mosq = mosquitto_new(cid.c_str(), true, nullptr); + if (!mosq) + { + throw std::runtime_error("Error creating MQTT client"); + } + + // std::cout << "[MQTT] Cliente criado com ID: " << cid << std::endl; + + // Configure callbacks + mosquitto_connect_callback_set(mosq, on_connect); + mosquitto_disconnect_callback_set(mosq, on_disconnect); + + // Configure log callback for debug + mosquitto_log_callback_set(mosq, [](struct mosquitto *mosq, void *userdata, int level, const char *str) + { + (void)mosq; // Evitar warning de parâmetro não utilizado + (void)userdata; // Avoid unused parameter warning + if (level <= MOSQ_LOG_WARNING) { // Only important logs + std::cout << "[MQTT LOG] " << str << std::endl; + } }); + + // Additional settings for stability + mosquitto_max_inflight_messages_set(mosq, 20); + mosquitto_message_retry_set(mosq, 3); + +// Configure authentication if necessary (for HiveMQ Cloud) +#ifdef MQTT_USERNAME + int auth_ret = mosquitto_username_pw_set(mosq, MQTT_USERNAME, MQTT_PASSWORD); + if (auth_ret != MOSQ_ERR_SUCCESS) + { + throw std::runtime_error("Error configuring MQTT authentication: " + std::string(mosquitto_strerror(auth_ret))); + } +#endif + + // Configure TLS for HiveMQ Cloud (port 8883) + if (MQTT_PORT == 8883) + { + // Use system CA bundle (safer than tls_insecure_set) + int tls_ret = mosquitto_tls_set(mosq, "/etc/ssl/certs/ca-certificates.crt", nullptr, nullptr, nullptr, nullptr); + if (tls_ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[WARNING] Falha ao configurar TLS com CA bundle: " << mosquitto_strerror(tls_ret) << std::endl; + std::cerr << "[WARNING] Tentando configuração TLS alternativa..." << std::endl; + + // Fallback: basic TLS configuration + tls_ret = mosquitto_tls_set(mosq, nullptr, nullptr, nullptr, nullptr, nullptr); + if (tls_ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[ERROR] Falha na configuração TLS alternativa: " << mosquitto_strerror(tls_ret) << std::endl; + } + } + else + { + // std::cout << "[MQTT] TLS configurado com CA bundle do sistema" << std::endl; + } + + // Note: TLS 1.2 is default in OpenSSL 1.1.1f + // std::cout << "[MQTT] TLS 1.2 será usado por padrão (OpenSSL 1.1.1f)" << std::endl; + + // Configure additional TLS options + tls_ret = mosquitto_tls_opts_set(mosq, 1, nullptr, nullptr); + if (tls_ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[WARNING] Falha ao configurar opções TLS: " << mosquitto_strerror(tls_ret) << std::endl; + } + + // std::cout << "[MQTT] Configuração TLS robusta aplicada para HiveMQ Cloud" << std::endl; + } + + // Configure optimized keepalive + int keepalive = 60; // 1 minute (optimized for stability) + + // Connect asynchronously for better performance + int ret = mosquitto_connect_async(mosq, MQTT_BROKER, MQTT_PORT, keepalive); + if (ret != MOSQ_ERR_SUCCESS) + { + throw std::runtime_error("Error starting MQTT async connection: " + std::string(mosquitto_strerror(ret))); + } + + // std::cout << "[MQTT] Iniciando conexão assíncrona ao broker MQTT em " << MQTT_BROKER << ":" << MQTT_PORT << " (keepalive: " << keepalive << "s)" << std::endl; + + // Use library loop (more efficient than custom thread) + ret = mosquitto_loop_start(mosq); + if (ret != MOSQ_ERR_SUCCESS) + { + throw std::runtime_error("Error starting MQTT loop: " + std::string(mosquitto_strerror(ret))); + } + + // std::cout << "[MQTT] Loop MQTT iniciado com sucesso" << std::endl; +} + +void cleanupMQTT() +{ + if (mosq) + { + // Parar o loop da biblioteca + mosquitto_loop_stop(mosq, true); + + // Desconectar e limpar + mosquitto_disconnect(mosq); + mosquitto_destroy(mosq); + mosq = nullptr; + } + + mosquitto_lib_cleanup(); + mqtt_connected = false; + mqtt_running = false; + // std::cout << "[MQTT] Conexão MQTT encerrada" << std::endl; +} + +// ====== MQTT RECONNECTION FUNCTION ====== +void reconnectMQTT() +{ + if (!mqtt_connected && mosq) + { + // std::cout << "[MQTT] Tentando reconectar..." << std::endl; + + // Wait a bit before trying to reconnect + std::this_thread::sleep_for(std::chrono::seconds(2)); + + // Check if client still exists and is valid + if (!mqtt_connected) + { + // std::cout << "[MQTT] Cliente não está conectado, tentando reconectar..." << std::endl; + + // Try reconnecting using async system + int ret = mosquitto_reconnect_async(mosq); + if (ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[MQTT] Falha na reconexão assíncrona: " << mosquitto_strerror(ret) << std::endl; + + // If it fails, try recreating the connection + // std::cout << "[MQTT] Tentando recriar conexão..." << std::endl; + + // Stop current loop + mosquitto_loop_stop(mosq, true); + mosquitto_disconnect(mosq); + mosquitto_destroy(mosq); + + // Recreate MQTT client with unique ID + std::string cid = "yolov8_detector_" + std::to_string(getpid()) + "_reconnect"; + mosq = mosquitto_new(cid.c_str(), true, nullptr); + if (mosq) + { + // Reconfigure callbacks + mosquitto_connect_callback_set(mosq, on_connect); + mosquitto_disconnect_callback_set(mosq, on_disconnect); + + // Reconfigure log callback + mosquitto_log_callback_set(mosq, [](struct mosquitto *mosq, void *userdata, int level, const char *str) + { + (void)mosq; // Evitar warning de parâmetro não utilizado + (void)userdata; // Avoid unused parameter warning + if (level <= MOSQ_LOG_WARNING) { + std::cout << "[MQTT LOG] " << str << std::endl; + } }); + +// Reconfigure authentication +#ifdef MQTT_USERNAME + mosquitto_username_pw_set(mosq, MQTT_USERNAME, MQTT_PASSWORD); +#endif + + // Reconfigure TLS + if (MQTT_PORT == 8883) + { + // Use system CA bundle + int tls_ret = mosquitto_tls_set(mosq, "/etc/ssl/certs/ca-certificates.crt", nullptr, nullptr, nullptr, nullptr); + if (tls_ret != MOSQ_ERR_SUCCESS) + { + std::cerr << "[WARNING] Falha ao reconfigurar TLS com CA bundle: " << mosquitto_strerror(tls_ret) << std::endl; + // Fallback + tls_ret = mosquitto_tls_set(mosq, nullptr, nullptr, nullptr, nullptr, nullptr); + } + + // TLS 1.2 is default in OpenSSL 1.1.1f + + // TLS options + mosquitto_tls_opts_set(mosq, 1, nullptr, nullptr); + } + + // Try connecting again asynchronously + ret = mosquitto_connect_async(mosq, MQTT_BROKER, MQTT_PORT, 60); + if (ret == MOSQ_ERR_SUCCESS) + { + // Start loop + ret = mosquitto_loop_start(mosq); + if (ret == MOSQ_ERR_SUCCESS) + { + // std::cout << "[MQTT] Reconexão bem-sucedida!" << std::endl; + } + else + { + std::cerr << "[MQTT] Falha ao iniciar loop após reconexão: " << mosquitto_strerror(ret) << std::endl; + } + } + else + { + std::cerr << "[MQTT] Falha na reconexão após recriação: " << mosquitto_strerror(ret) << std::endl; + } + } + } + else + { + // std::cout << "[MQTT] Reconexão assíncrona iniciada..." << std::endl; + } + } + else + { + // std::cout << "[MQTT] Cliente ainda está conectado, verificando status..." << std::endl; + // Verificar se realmente está conectado + if (mqtt_connected) + { + // std::cout << "[MQTT] Cliente reconectado com sucesso!" << std::endl; + } + } + } +} + +// ====== STREAMING FUNCTIONS ====== +bool initStreaming() +{ + try + { + // UDP streaming pipeline (streaming only, no camera capture) + std::string stream_pipeline = + "appsrc ! videoconvert ! " + "x264enc tune=zerolatency bitrate=2000 speed-preset=superfast ! " + "rtph264pay ! udpsink host=" + + std::string(STREAM_IP) + + " port=" + std::to_string(STREAM_PORT) + " sync=false"; + + stream_writer.open(stream_pipeline, cv::CAP_GSTREAMER, 0, STREAM_FPS, + cv::Size(STREAM_WIDTH, STREAM_HEIGHT), true); + if (!stream_writer.isOpened()) + { + std::cerr << "[STREAMING] Error opening UDP stream" << std::endl; + return false; + } + + streaming_initialized = true; + std::cout << "[STREAMING] Initialized successfully!" << std::endl; + std::cout << "[STREAMING] Streaming masks to " << STREAM_IP << ":" << STREAM_PORT << std::endl; + std::cout << "[STREAMING] Resolution: " << STREAM_WIDTH << "x" << STREAM_HEIGHT << " @ " << STREAM_FPS << "fps" << std::endl; + return true; + } + catch (const std::exception &e) + { + std::cerr << "[STREAMING] Initialization error: " << e.what() << std::endl; + return false; + } +} + +void cleanupStreaming() +{ + if (streaming_initialized) + { + if (stream_writer.isOpened()) + { + stream_writer.release(); + } + streaming_initialized = false; + std::cout << "[STREAMING] Resources released" << std::endl; + } +} + +void signal_handler(int) +{ + std::cout << std::endl + << "[!] Ctrl+C detected. Stopping the JetRacer..." << std::endl; + program_running = false; + if (jetracer_ptr) + jetracer_ptr->stop(); + cleanupStreaming(); + cleanupMQTT(); + std::_Exit(0); +} + +int main() +{ + std::cout << "=== PID with joystick (manual speed) + shared memory ===" << std::endl; + + try + { + jetracer::control::JetRacer jetracer(0x40, 0x60); + jetracer_ptr = &jetracer; + + signal(SIGINT, signal_handler); + + // ===== MODO DE JOYSTICK ATIVADO ===== + jetracer.set_constant_speed_mode(false); // ← ATIVAR MODO DE JOYSTICK + + std::cout << "\n=== MODO DE JOYSTICK ATIVADO ===" << std::endl; + std::cout << "Controle via joystick habilitado" << std::endl; + std::cout << "Velocidade máxima limitada a 27% (configuração ideal)" << std::endl; + std::cout << "Use o joystick esquerdo para controlar a velocidade" << std::endl; + + // Iniciar o sistema principal + jetracer.start(); + + int shm_fd = shm_open("mask_shared", O_RDWR, 0666); + if (shm_fd == -1) + { + std::cerr << "Error oppening shared memory." << std::endl; + return 1; + } + + uint8_t *shm_ptr = (uint8_t *)mmap(nullptr, 2 * jetracer::vision::SIZE + 1, PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); + if (shm_ptr == MAP_FAILED) + { + std::cerr << "Error mapping memory." << std::endl; + return 1; + } + + // Estrutura da shared memory: flag + lane_mask + drivable_mask + uint8_t *flag_ptr = shm_ptr; + uint8_t *lane_mask_ptr = shm_ptr + 1; + uint8_t *drivable_mask_ptr = shm_ptr + 1 + jetracer::vision::SIZE; + + cv::Mat lane_mask(jetracer::vision::HEIGHT, jetracer::vision::WIDTH, CV_8UC1, lane_mask_ptr); + cv::Mat drivable_mask(jetracer::vision::HEIGHT, jetracer::vision::WIDTH, CV_8UC1, drivable_mask_ptr); + + // Note: drivable_mask contains the drivable area detected by YOLO + // Can be used for additional validation, navigation or terrain analysis + + std::cout << "\n=== SISTEMA PRINCIPAL EM EXECUÇÃO ===" << std::endl; + std::cout << "Controle via joystick ativo" << std::endl; + std::cout << "Velocidade máxima limitada a 27% (configuração ideal)" << std::endl; + std::cout << "Use o joystick esquerdo para controlar a velocidade" << std::endl; + std::cout << "Recebendo máscaras: Lane + Drivable" << std::endl; + std::cout << "\n=== CRUISE CONTROL (MODO AUTÔNOMO) ===" << std::endl; + std::cout << "Botão R2: Ativar/Desativar cruise control" << std::endl; + std::cout << "• Pressione R2 para manter a velocidade atual" << std::endl; + std::cout << "• Pressione R2 novamente para voltar ao controle manual" << std::endl; + std::cout << "• Cruise control é desativado automaticamente em emergência" << std::endl; + std::cout << "\nUse Ctrl+C para parar o sistema a qualquer momento" << std::endl; + + // ====== MQTT INITIALIZATION ====== + try + { + initMQTT(); + std::cout << "[INFO] MQTT inicializado com sucesso!" << std::endl; + } + catch (const std::exception &e) + { + std::cerr << "[ERROR] Falha ao inicializar MQTT: " << e.what() << std::endl; + std::cerr << "[WARNING] Continuando sem MQTT..." << std::endl; + } + + // ====== STREAMING INITIALIZATION ====== + if (!initStreaming()) + { + std::cerr << "[ERROR] Falha ao inicializar streaming!" << std::endl; + std::cerr << "[WARNING] Continuando sem streaming..." << std::endl; + } + + // Variables for streaming + auto start_time = std::chrono::steady_clock::now(); + int stream_fps_counter = 0; + float stream_fps = 0.0f; + + while (program_running) + { + + if (flag_ptr[0] != 1) + { + usleep(3000); + continue; + } + + // ====== CHECK AND RECONNECT MQTT IF NECESSARY ====== + static int frame_counter = 0; + if (!mqtt_connected && frame_counter % 30 == 0) + { // Try reconnecting every 30 frames + // std::cout << "[MQTT] Status: Desconectado - tentando reconectar..." << std::endl; + + // Check if MQTT client is still valid + if (mosq && mqtt_connected) + { + // std::cout << "[MQTT] Cliente ainda está conectado, atualizando status..." << std::endl; + } + else + { + reconnectMQTT(); + } + } + + // ====== DETECT AND PUBLISH SPECIFIC CLASSES VIA MQTT ====== + // detectAndPublishClasses(res, labels_map); // Comentado temporariamente + + // ====== EMERGENCY STOP SYSTEM ====== + // Detect lane curves + float y_ref; + std::vector left_curve, right_curve; + bool lanes_detected = jetracer::vision::extractLanePoints(lane_mask, lane_mask.cols / 2.0f, y_ref, left_curve, right_curve); + + // ====== INTEGRATED EMERGENCY STOP SYSTEM ====== + float lane_occupancy = 0.0f; + float drivable_occupancy = 0.0f; + cv::Mat danger_zone_mask, drivable_danger_zone_mask; + bool emergency_triggered = false; + std::string emergency_source = ""; + + // ====== ZONA DE PERIGO DAS LANES ====== + if (lanes_detected) + { + // Criar máscara da zona de perigo para cálculo de ocupação + const float scale = 40.0f / (lane_mask.cols / 2.0f); + jetracer::vision::createDangerZoneMask(lane_mask, left_curve, right_curve, 17.0f, scale, danger_zone_mask); + + // Calcular ocupação da zona de perigo usando a máscara da zona de perigo + lane_occupancy = jetracer::vision::calculateDangerZoneOccupancyFromMask(lane_mask, danger_zone_mask); + } + + // ====== ZONA DE PERIGO DRIVABLE ====== + // Sempre processar zona de perigo drivable (mesmo se máscara estiver vazia) + const float scale = 40.0f / (drivable_mask.cols / 2.0f); + jetracer::vision::createDrivableDangerZoneMask(drivable_mask, 17.0f, scale, drivable_danger_zone_mask); + drivable_occupancy = jetracer::vision::calculateDrivableDangerZoneOccupancy(drivable_mask, drivable_danger_zone_mask); + + // Log detalhado da ocupação drivable + if (drivable_occupancy > 0.0f) + { + std::cout << "[DRIVABLE] Ocupação na zona de perigo: " << (drivable_occupancy * 100) << "%" << std::endl; + } + + // Log de debug para verificar se a zona de perigo está sendo criada + if (cv::countNonZero(drivable_danger_zone_mask) > 0) + { + std::cout << "[DEBUG] Zona de perigo drivable criada com " << cv::countNonZero(drivable_danger_zone_mask) << " pixels" << std::endl; + } + + // Log de debug para verificar o estado das máscaras + int drivable_pixels = cv::countNonZero(drivable_mask); + std::cout << "[DEBUG] Máscara drivable: " << drivable_pixels << " pixels, Zona de perigo: " << cv::countNonZero(drivable_danger_zone_mask) << " pixels" << std::endl; + + // ====== VERIFICAÇÃO INTEGRADA DE EMERGÊNCIA ====== + // Verificar se qualquer zona de perigo excede o limiar (25%) + // Prioridade: LANES primeiro, depois DRIVABLE + if (lane_occupancy > EMERGENCY_THRESHOLD) + { + emergency_triggered = true; + emergency_source = "LANES"; + std::cout << "[EMERGENCY CHECK] Zona de perigo LANES excedeu limiar: " << (lane_occupancy * 100) << "% > " << (EMERGENCY_THRESHOLD * 100) << "%" << std::endl; + } + else if (drivable_occupancy > EMERGENCY_THRESHOLD) + { + emergency_triggered = true; + emergency_source = "DRIVABLE"; + std::cout << "[EMERGENCY CHECK] Zona de perigo DRIVABLE excedeu limiar: " << (drivable_occupancy * 100) << "% > " << (EMERGENCY_THRESHOLD * 100) << "%" << std::endl; + } + + // Ativar parada de emergência se necessário + if (emergency_triggered && !emergency_stop_active) + { + // Salvar estado antes da emergência + cruise_control_before_emergency = jetracer.is_cruise_control_active(); + if (cruise_control_before_emergency) + { + // Se estava em cruise control, salvar a velocidade do cruise control + speed_before_emergency = jetracer.get_cruise_control_speed(); + } + else + { + // Se estava em modo manual, precisamos obter a velocidade atual do joystick + // Como não temos acesso direto ao joystick aqui, vamos usar uma abordagem diferente + // Vamos salvar a velocidade atual do sistema (que pode ser obtida de outras formas) + speed_before_emergency = 0.0f; // Será definida pelo usuário após a recuperação + } + + emergency_stop_active = true; + emergency_stop_time = std::chrono::steady_clock::now(); + jetracer.emergency_stop(); + std::cout << "[EMERGENCY] Zona de perigo detectada em " << emergency_source << "! "; + if (emergency_source == "LANES") + { + std::cout << "Ocupação lanes: " << (lane_occupancy * 100) << "%"; + } + else + { + std::cout << "Ocupação drivable: " << (drivable_occupancy * 100) << "%"; + } + if (cruise_control_before_emergency) + { + std::cout << " - Cruise control ativo, velocidade salva: " << speed_before_emergency << "%"; + } + else + { + std::cout << " - Modo manual, velocidade será restaurada pelo joystick"; + } + std::cout << std::endl; + } + // Verificar se pode retomar o controle (ambas as zonas devem estar livres) + else if (!emergency_triggered && emergency_stop_active) + { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - emergency_stop_time).count(); + + if (elapsed >= EMERGENCY_RECOVERY_DELAY_MS) + { + emergency_stop_active = false; + + // LIBERAR TRAVAMENTO DO MOTOR + jetracer.release_motor_lock(); + + // Restaurar velocidade e estado do cruise control + if (cruise_control_before_emergency && std::abs(speed_before_emergency) > 5.0f) + { + // Restaurar cruise control com velocidade salva + jetracer.set_cruise_control_speed(speed_before_emergency); + jetracer.set_cruise_control_mode(true); + std::cout << "[EMERGENCY] Zonas livres! Restaurando cruise control com velocidade: " << speed_before_emergency << "%" << std::endl; + } + else if (!cruise_control_before_emergency) + { + // Modo manual - o joystick retomará o controle automaticamente + std::cout << "[EMERGENCY] Zonas livres! Retomando controle manual via joystick." << std::endl; + } + else + { + std::cout << "[EMERGENCY] Zonas livres! Retomando controle normal (velocidade baixa não restaurada)." << std::endl; + } + + // Resetar variáveis de estado + speed_before_emergency = 0.0f; + cruise_control_before_emergency = false; + } + } + + // Executar PID apenas se não estiver em parada de emergência + float pid_angle = 0.0f; + if (!emergency_stop_active) + { + pid_angle = jetracer::pid::PIDexecute(lane_mask.clone()); + jetracer.smooth_steering(static_cast(pid_angle), 5); + } + + frame_counter++; + + // ====== STREAMING DAS MÁSCARAS (ORIGINAL + ZONA DE PERIGO) ====== + if (streaming_initialized && stream_writer.isOpened()) + { + cv::Mat mask_for_stream = lane_mask.clone(); + + // Construir curvas dinâmicas na metade inferior + const float image_center = mask_for_stream.cols / 2.0f; + const float scale = 40.0f / (mask_for_stream.cols / 2.0f); // mantém a tua conversão cm->px + std::vector left_curve, right_curve; + + // Opcional: um pouco de fecho morfológico ajuda em falhas pequenas + // cv::morphologyEx(mask_for_stream, mask_for_stream, cv::MORPH_CLOSE, + // cv::getStructuringElement(cv::MORPH_RECT, {3,3})); + + jetracer::vision::sampleLaneEdgesByRow( + mask_for_stream, + mask_for_stream.rows * 2 / 5, // y_start (3/5 inferiores) + mask_for_stream.rows, // y_end + 2, // step em píxeis (aumenta para +fps) + image_center, + left_curve, right_curve, 3 // min_run + ); + + // Zona de perigo dinâmica na máscara original + jetracer::vision::drawDangerZoneCurved(mask_for_stream, left_curve, right_curve, 17.0f, scale); + + // Criar máscara separada apenas da zona de perigo + cv::Mat danger_zone_mask; + jetracer::vision::createDangerZoneMask(lane_mask, left_curve, right_curve, 17.0f, scale, danger_zone_mask); + + // Converter máscaras para formato visual + cv::Mat lane_visual, drivable_visual, danger_zone_visual; + cv::cvtColor(mask_for_stream, lane_visual, cv::COLOR_GRAY2BGR); + cv::cvtColor(drivable_mask, drivable_visual, cv::COLOR_GRAY2BGR); + cv::cvtColor(danger_zone_mask, danger_zone_visual, cv::COLOR_GRAY2BGR); + + // Redimensionar máscaras para terço da largura do stream + int third_width = STREAM_WIDTH / 3; + cv::Mat lane_resized, drivable_resized, danger_zone_resized; + cv::resize(lane_visual, lane_resized, cv::Size(third_width, STREAM_HEIGHT)); + cv::resize(drivable_visual, drivable_resized, cv::Size(third_width, STREAM_HEIGHT)); + cv::resize(danger_zone_visual, danger_zone_resized, cv::Size(third_width, STREAM_HEIGHT)); + + // Criar imagem combinada (3 colunas) + cv::Mat combined_frame = cv::Mat::zeros(STREAM_HEIGHT, STREAM_WIDTH, CV_8UC3); + + // Colocar máscara lane à esquerda + cv::Rect left_roi(0, 0, third_width, STREAM_HEIGHT); + lane_resized.copyTo(combined_frame(left_roi)); + + // Colocar máscara drivable no centro + cv::Rect center_roi(third_width, 0, third_width, STREAM_HEIGHT); + drivable_resized.copyTo(combined_frame(center_roi)); + + // Colocar máscara da zona de perigo à direita + cv::Rect right_roi(2 * third_width, 0, third_width, STREAM_HEIGHT); + danger_zone_resized.copyTo(combined_frame(right_roi)); + + // HUD + stream_fps_counter++; + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - start_time).count(); + if (elapsed >= 1) + { + stream_fps = stream_fps_counter / (float)elapsed; + start_time = now; + stream_fps_counter = 0; + } + + // Adicionar textos informativos + cv::putText(combined_frame, "FPS: " + std::to_string((int)stream_fps), {10, 30}, + cv::FONT_HERSHEY_SIMPLEX, 0.8, {0, 255, 0}, 2); + cv::putText(combined_frame, "TRIPLE MASK STREAMING", {10, 60}, + cv::FONT_HERSHEY_SIMPLEX, 0.8, {0, 255, 0}, 2); + cv::putText(combined_frame, "PID: " + std::to_string(pid_angle), {10, 90}, + cv::FONT_HERSHEY_SIMPLEX, 0.8, {0, 255, 0}, 2); + + // Indicador do Cruise Control + if (jetracer.is_cruise_control_active()) + { + cv::putText(combined_frame, "CRUISE CONTROL: ATIVO", {10, 120}, + cv::FONT_HERSHEY_SIMPLEX, 0.8, {0, 255, 255}, 2); + cv::putText(combined_frame, "VELOCIDADE: " + std::to_string((int)jetracer.get_cruise_control_speed()) + "%", {10, 150}, + cv::FONT_HERSHEY_SIMPLEX, 0.8, {0, 255, 255}, 2); + } + else + { + cv::putText(combined_frame, "CRUISE CONTROL: INATIVO", {10, 120}, + cv::FONT_HERSHEY_SIMPLEX, 0.8, {128, 128, 128}, 2); + } + + // Labels para as máscaras + cv::putText(combined_frame, "LANE MASK", {10, STREAM_HEIGHT - 30}, + cv::FONT_HERSHEY_SIMPLEX, 0.7, {255, 255, 255}, 2); + cv::putText(combined_frame, "DRIVABLE MASK", {third_width + 10, STREAM_HEIGHT - 30}, + cv::FONT_HERSHEY_SIMPLEX, 0.7, {0, 255, 255}, 2); + cv::putText(combined_frame, "DANGER ZONE", {2 * third_width + 10, STREAM_HEIGHT - 30}, + cv::FONT_HERSHEY_SIMPLEX, 0.7, {0, 0, 255}, 2); + + // ====== INDICADORES INTEGRADOS DE ZONA DE PERIGO ====== + // Indicador das lanes + if (lanes_detected) + { + float lane_occupancy_percent = lane_occupancy * 100.0f; + + // Determinar cor baseada na ocupação das lanes + cv::Scalar lane_color; + std::string lane_status_text; + + if (lane_occupancy_percent < 15.0f) + { + lane_color = cv::Scalar(0, 255, 0); // Verde - seguro + lane_status_text = "SEGURO"; + } + else if (lane_occupancy_percent < 25.0f) + { + lane_color = cv::Scalar(0, 255, 255); // Amarelo - atenção + lane_status_text = "ATENCAO"; + } + else + { + lane_color = cv::Scalar(0, 0, 255); // Vermelho - emergência + lane_status_text = "EMERGENCIA"; + } + + // Exibir status da zona de perigo das lanes + cv::putText(combined_frame, "LANES: " + lane_status_text, + cv::Point(10, 120), cv::FONT_HERSHEY_SIMPLEX, 0.8, lane_color, 2); + cv::putText(combined_frame, "Ocupacao: " + std::to_string((int)lane_occupancy_percent) + "%", + cv::Point(10, 150), cv::FONT_HERSHEY_SIMPLEX, 0.8, lane_color, 2); + } + else + { + // Se não detectar faixas, mostrar aviso + cv::putText(combined_frame, "LANES: NAO DETECTADAS", + cv::Point(10, 120), cv::FONT_HERSHEY_SIMPLEX, 0.8, + cv::Scalar(0, 0, 255), 2); + } + + // ====== INDICADORES DE ZONA DE PERIGO DRIVABLE ====== + if (drivable_occupancy > 0.0f) + { + float drivable_occupancy_percent = drivable_occupancy * 100.0f; + + // Determinar cor baseada na ocupação drivable + cv::Scalar drivable_color; + std::string drivable_status_text; + + if (drivable_occupancy_percent < 15.0f) + { + drivable_color = cv::Scalar(0, 255, 0); // Verde - seguro + drivable_status_text = "SEGURO"; + } + else if (drivable_occupancy_percent < 25.0f) + { + drivable_color = cv::Scalar(0, 255, 255); // Amarelo - atenção + drivable_status_text = "ATENCAO"; + } + else + { + drivable_color = cv::Scalar(0, 0, 255); // Vermelho - perigo + drivable_status_text = "EMERGENCIA"; + } + + // Exibir status da zona de perigo drivable + cv::putText(combined_frame, "DRIVABLE: " + drivable_status_text, + cv::Point(10, 180), cv::FONT_HERSHEY_SIMPLEX, 0.8, drivable_color, 2); + cv::putText(combined_frame, "Ocupacao: " + std::to_string((int)drivable_occupancy_percent) + "%", + cv::Point(10, 210), cv::FONT_HERSHEY_SIMPLEX, 0.8, drivable_color, 2); + } + else + { + // Se não há área drivable detectada + cv::putText(combined_frame, "DRIVABLE: NAO DETECTADO", + cv::Point(10, 180), cv::FONT_HERSHEY_SIMPLEX, 0.8, + cv::Scalar(128, 128, 128), 2); + } + + // ====== MENSAGEM DE EMERGÊNCIA INTEGRADA ====== + if (emergency_stop_active) + { + cv::putText(combined_frame, "EMERGENCY STOP!", + cv::Point(10, 240), cv::FONT_HERSHEY_SIMPLEX, 1.0, + cv::Scalar(0, 0, 255), 3); + + // Mostrar qual zona causou a emergência + if (emergency_source == "LANES") + { + cv::putText(combined_frame, "CAUSA: ZONA DE PERIGO LANES", + cv::Point(10, 270), cv::FONT_HERSHEY_SIMPLEX, 0.7, + cv::Scalar(0, 0, 255), 2); + } + else if (emergency_source == "DRIVABLE") + { + cv::putText(combined_frame, "CAUSA: ZONA DE PERIGO DRIVABLE", + cv::Point(10, 270), cv::FONT_HERSHEY_SIMPLEX, 0.7, + cv::Scalar(0, 0, 255), 2); + } + + // Mostrar informações sobre recuperação + if (cruise_control_before_emergency && std::abs(speed_before_emergency) > 5.0f) + { + cv::putText(combined_frame, "VELOCIDADE SALVA: " + std::to_string((int)speed_before_emergency) + "%", + cv::Point(10, 300), cv::FONT_HERSHEY_SIMPLEX, 0.7, + cv::Scalar(0, 255, 255), 2); + cv::putText(combined_frame, "CRUISE CONTROL SERA RESTAURADO", + cv::Point(10, 330), cv::FONT_HERSHEY_SIMPLEX, 0.7, + cv::Scalar(0, 255, 255), 2); + } + else if (!cruise_control_before_emergency) + { + cv::putText(combined_frame, "CONTROLE MANUAL SERA RESTAURADO", + cv::Point(10, 300), cv::FONT_HERSHEY_SIMPLEX, 0.7, + cv::Scalar(0, 255, 255), 2); + } + } + + stream_writer.write(combined_frame); + } + + flag_ptr[0] = 0; + + if (cv::waitKey(1) == 27) + break; + } + + // Sistema em modo de joystick - não há teste para parar + + jetracer.stop(); + cleanupStreaming(); + cleanupMQTT(); + std::cout << "Finishing." << std::endl; + return 0; + } + catch (const std::exception &e) + { + std::cerr << "[ERROR] " << e.what() << std::endl; + if (jetracer_ptr) + jetracer_ptr->stop(); + cleanupStreaming(); + cleanupMQTT(); + return 1; + } +} diff --git a/emergency_stop/bin/jetracer_pid_controler b/emergency_stop/bin/jetracer_pid_controler new file mode 100755 index 000000000..bec246dfb Binary files /dev/null and b/emergency_stop/bin/jetracer_pid_controler differ diff --git a/emergency_stop/build/computer_vision.d b/emergency_stop/build/computer_vision.d new file mode 100644 index 000000000..8944caecd --- /dev/null +++ b/emergency_stop/build/computer_vision.d @@ -0,0 +1,113 @@ +build/computer_vision.o: sources/computer_vision.cpp \ + includes/jetracer/computer_vision.hpp \ + /usr/include/opencv4/opencv2/opencv.hpp \ + /usr/include/opencv4/opencv2/opencv_modules.hpp \ + /usr/include/opencv4/opencv2/core.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/core/version.hpp \ + /usr/include/opencv4/opencv2/core/hal/interface.h \ + /usr/include/opencv4/opencv2/core/cv_cpu_dispatch.h \ + /usr/include/opencv4/opencv2/core/base.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.hpp \ + /usr/include/opencv4/opencv2/core/cvstd_wrapper.hpp \ + /usr/include/opencv4/opencv2/core/neon_utils.hpp \ + /usr/include/opencv4/opencv2/core/vsx_utils.hpp \ + /usr/include/opencv4/opencv2/core/check.hpp \ + /usr/include/opencv4/opencv2/core/traits.hpp \ + /usr/include/opencv4/opencv2/core/matx.hpp \ + /usr/include/opencv4/opencv2/core/saturate.hpp \ + /usr/include/opencv4/opencv2/core/fast_math.hpp \ + /usr/include/opencv4/opencv2/core/types.hpp \ + /usr/include/opencv4/opencv2/core/mat.hpp \ + /usr/include/opencv4/opencv2/core/bufferpool.hpp \ + /usr/include/opencv4/opencv2/core/mat.inl.hpp \ + /usr/include/opencv4/opencv2/core/persistence.hpp \ + /usr/include/opencv4/opencv2/core/operations.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.inl.hpp \ + /usr/include/opencv4/opencv2/core/utility.hpp \ + /usr/include/opencv4/opencv2/core/optim.hpp \ + /usr/include/opencv4/opencv2/core/ovx.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/calib3d.hpp \ + /usr/include/opencv4/opencv2/features2d.hpp \ + /usr/include/opencv4/opencv2/flann/miniflann.hpp \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/config.h \ + /usr/include/opencv4/opencv2/core/affine.hpp \ + /usr/include/opencv4/opencv2/dnn.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.hpp \ + /usr/include/opencv4/opencv2/core/async.hpp \ + /usr/include/opencv4/opencv2/dnn/../dnn/version.hpp \ + /usr/include/opencv4/opencv2/dnn/dict.hpp \ + /usr/include/opencv4/opencv2/dnn/layer.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.inl.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/inference_engine.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/../dnn.hpp \ + /usr/include/opencv4/opencv2/flann.hpp \ + /usr/include/opencv4/opencv2/flann/flann_base.hpp \ + /usr/include/opencv4/opencv2/flann/general.h \ + /usr/include/opencv4/opencv2/flann/matrix.h \ + /usr/include/opencv4/opencv2/flann/params.h \ + /usr/include/opencv4/opencv2/flann/any.h \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/saving.h \ + /usr/include/opencv4/opencv2/flann/nn_index.h \ + /usr/include/opencv4/opencv2/flann/result_set.h \ + /usr/include/opencv4/opencv2/flann/all_indices.h \ + /usr/include/opencv4/opencv2/flann/kdtree_index.h \ + /usr/include/opencv4/opencv2/flann/dynamic_bitset.h \ + /usr/include/opencv4/opencv2/flann/dist.h \ + /usr/include/opencv4/opencv2/flann/heap.h \ + /usr/include/opencv4/opencv2/flann/allocator.h \ + /usr/include/opencv4/opencv2/flann/random.h \ + /usr/include/opencv4/opencv2/flann/kdtree_single_index.h \ + /usr/include/opencv4/opencv2/flann/kmeans_index.h \ + /usr/include/opencv4/opencv2/flann/logger.h \ + /usr/include/opencv4/opencv2/flann/composite_index.h \ + /usr/include/opencv4/opencv2/flann/linear_index.h \ + /usr/include/opencv4/opencv2/flann/hierarchical_clustering_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_table.h \ + /usr/include/opencv4/opencv2/flann/autotuned_index.h \ + /usr/include/opencv4/opencv2/flann/ground_truth.h \ + /usr/include/opencv4/opencv2/flann/index_testing.h \ + /usr/include/opencv4/opencv2/flann/timer.h \ + /usr/include/opencv4/opencv2/flann/sampling.h \ + /usr/include/opencv4/opencv2/highgui.hpp \ + /usr/include/opencv4/opencv2/imgcodecs.hpp \ + /usr/include/opencv4/opencv2/videoio.hpp \ + /usr/include/opencv4/opencv2/imgproc.hpp \ + /usr/include/opencv4/opencv2/./imgproc/segmentation.hpp \ + /usr/include/opencv4/opencv2/ml.hpp \ + /usr/include/opencv4/opencv2/ml/ml.inl.hpp \ + /usr/include/opencv4/opencv2/objdetect.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_dictionary.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_board.hpp \ + /usr/include/opencv4/opencv2/objdetect/graphical_code_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/detection_based_tracker.hpp \ + /usr/include/opencv4/opencv2/objdetect/face.hpp \ + /usr/include/opencv4/opencv2/objdetect/charuco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/barcode.hpp \ + /usr/include/opencv4/opencv2/photo.hpp \ + /usr/include/opencv4/opencv2/stitching.hpp \ + /usr/include/opencv4/opencv2/stitching/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/core/cuda.hpp \ + /usr/include/opencv4/opencv2/core/cuda_types.hpp \ + /usr/include/opencv4/opencv2/core/cuda.inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/exposure_compensate.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/seam_finders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/blenders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/video.hpp \ + /usr/include/opencv4/opencv2/video/tracking.hpp \ + /usr/include/opencv4/opencv2/video/background_segm.hpp diff --git a/emergency_stop/build/computer_vision.o b/emergency_stop/build/computer_vision.o new file mode 100644 index 000000000..375dd7328 Binary files /dev/null and b/emergency_stop/build/computer_vision.o differ diff --git a/emergency_stop/build/i2c_device.d b/emergency_stop/build/i2c_device.d new file mode 100644 index 000000000..9b1de98c4 --- /dev/null +++ b/emergency_stop/build/i2c_device.d @@ -0,0 +1,2 @@ +build/i2c_device.o: sources/i2c_device.cpp \ + includes/jetracer/i2c_device.hpp diff --git a/emergency_stop/build/i2c_device.o b/emergency_stop/build/i2c_device.o new file mode 100644 index 000000000..1975a19e2 Binary files /dev/null and b/emergency_stop/build/i2c_device.o differ diff --git a/emergency_stop/build/jetracer.d b/emergency_stop/build/jetracer.d new file mode 100644 index 000000000..e0225d23e --- /dev/null +++ b/emergency_stop/build/jetracer.d @@ -0,0 +1,3 @@ +build/jetracer.o: sources/jetracer.cpp includes/jetracer/jetracer.hpp \ + includes/jetracer/i2c_device.hpp includes/jetracer/pwm_config.hpp \ + includes/jetracer/motor_control.hpp diff --git a/emergency_stop/build/jetracer.o b/emergency_stop/build/jetracer.o new file mode 100644 index 000000000..8b990ef06 Binary files /dev/null and b/emergency_stop/build/jetracer.o differ diff --git a/emergency_stop/build/main.d b/emergency_stop/build/main.d new file mode 100644 index 000000000..66148e6e6 --- /dev/null +++ b/emergency_stop/build/main.d @@ -0,0 +1,115 @@ +build/main.o: apps/main.cpp includes/jetracer/pid_controller.hpp \ + /usr/include/opencv4/opencv2/opencv.hpp \ + /usr/include/opencv4/opencv2/opencv_modules.hpp \ + /usr/include/opencv4/opencv2/core.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/core/version.hpp \ + /usr/include/opencv4/opencv2/core/hal/interface.h \ + /usr/include/opencv4/opencv2/core/cv_cpu_dispatch.h \ + /usr/include/opencv4/opencv2/core/base.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.hpp \ + /usr/include/opencv4/opencv2/core/cvstd_wrapper.hpp \ + /usr/include/opencv4/opencv2/core/neon_utils.hpp \ + /usr/include/opencv4/opencv2/core/vsx_utils.hpp \ + /usr/include/opencv4/opencv2/core/check.hpp \ + /usr/include/opencv4/opencv2/core/traits.hpp \ + /usr/include/opencv4/opencv2/core/matx.hpp \ + /usr/include/opencv4/opencv2/core/saturate.hpp \ + /usr/include/opencv4/opencv2/core/fast_math.hpp \ + /usr/include/opencv4/opencv2/core/types.hpp \ + /usr/include/opencv4/opencv2/core/mat.hpp \ + /usr/include/opencv4/opencv2/core/bufferpool.hpp \ + /usr/include/opencv4/opencv2/core/mat.inl.hpp \ + /usr/include/opencv4/opencv2/core/persistence.hpp \ + /usr/include/opencv4/opencv2/core/operations.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.inl.hpp \ + /usr/include/opencv4/opencv2/core/utility.hpp \ + /usr/include/opencv4/opencv2/core/optim.hpp \ + /usr/include/opencv4/opencv2/core/ovx.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/calib3d.hpp \ + /usr/include/opencv4/opencv2/features2d.hpp \ + /usr/include/opencv4/opencv2/flann/miniflann.hpp \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/config.h \ + /usr/include/opencv4/opencv2/core/affine.hpp \ + /usr/include/opencv4/opencv2/dnn.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.hpp \ + /usr/include/opencv4/opencv2/core/async.hpp \ + /usr/include/opencv4/opencv2/dnn/../dnn/version.hpp \ + /usr/include/opencv4/opencv2/dnn/dict.hpp \ + /usr/include/opencv4/opencv2/dnn/layer.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.inl.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/inference_engine.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/../dnn.hpp \ + /usr/include/opencv4/opencv2/flann.hpp \ + /usr/include/opencv4/opencv2/flann/flann_base.hpp \ + /usr/include/opencv4/opencv2/flann/general.h \ + /usr/include/opencv4/opencv2/flann/matrix.h \ + /usr/include/opencv4/opencv2/flann/params.h \ + /usr/include/opencv4/opencv2/flann/any.h \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/saving.h \ + /usr/include/opencv4/opencv2/flann/nn_index.h \ + /usr/include/opencv4/opencv2/flann/result_set.h \ + /usr/include/opencv4/opencv2/flann/all_indices.h \ + /usr/include/opencv4/opencv2/flann/kdtree_index.h \ + /usr/include/opencv4/opencv2/flann/dynamic_bitset.h \ + /usr/include/opencv4/opencv2/flann/dist.h \ + /usr/include/opencv4/opencv2/flann/heap.h \ + /usr/include/opencv4/opencv2/flann/allocator.h \ + /usr/include/opencv4/opencv2/flann/random.h \ + /usr/include/opencv4/opencv2/flann/kdtree_single_index.h \ + /usr/include/opencv4/opencv2/flann/kmeans_index.h \ + /usr/include/opencv4/opencv2/flann/logger.h \ + /usr/include/opencv4/opencv2/flann/composite_index.h \ + /usr/include/opencv4/opencv2/flann/linear_index.h \ + /usr/include/opencv4/opencv2/flann/hierarchical_clustering_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_table.h \ + /usr/include/opencv4/opencv2/flann/autotuned_index.h \ + /usr/include/opencv4/opencv2/flann/ground_truth.h \ + /usr/include/opencv4/opencv2/flann/index_testing.h \ + /usr/include/opencv4/opencv2/flann/timer.h \ + /usr/include/opencv4/opencv2/flann/sampling.h \ + /usr/include/opencv4/opencv2/highgui.hpp \ + /usr/include/opencv4/opencv2/imgcodecs.hpp \ + /usr/include/opencv4/opencv2/videoio.hpp \ + /usr/include/opencv4/opencv2/imgproc.hpp \ + /usr/include/opencv4/opencv2/./imgproc/segmentation.hpp \ + /usr/include/opencv4/opencv2/ml.hpp \ + /usr/include/opencv4/opencv2/ml/ml.inl.hpp \ + /usr/include/opencv4/opencv2/objdetect.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_dictionary.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_board.hpp \ + /usr/include/opencv4/opencv2/objdetect/graphical_code_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/detection_based_tracker.hpp \ + /usr/include/opencv4/opencv2/objdetect/face.hpp \ + /usr/include/opencv4/opencv2/objdetect/charuco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/barcode.hpp \ + /usr/include/opencv4/opencv2/photo.hpp \ + /usr/include/opencv4/opencv2/stitching.hpp \ + /usr/include/opencv4/opencv2/stitching/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/core/cuda.hpp \ + /usr/include/opencv4/opencv2/core/cuda_types.hpp \ + /usr/include/opencv4/opencv2/core/cuda.inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/exposure_compensate.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/seam_finders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/blenders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/video.hpp \ + /usr/include/opencv4/opencv2/video/tracking.hpp \ + /usr/include/opencv4/opencv2/video/background_segm.hpp \ + includes/jetracer/jetracer.hpp includes/jetracer/i2c_device.hpp \ + includes/jetracer/pwm_config.hpp includes/jetracer/motor_control.hpp \ + includes/jetracer/computer_vision.hpp diff --git a/emergency_stop/build/main.o b/emergency_stop/build/main.o new file mode 100644 index 000000000..a6ca0e6aa Binary files /dev/null and b/emergency_stop/build/main.o differ diff --git a/emergency_stop/build/pid_controller.d b/emergency_stop/build/pid_controller.d new file mode 100644 index 000000000..28eaa6813 --- /dev/null +++ b/emergency_stop/build/pid_controller.d @@ -0,0 +1,114 @@ +build/pid_controller.o: sources/pid_controller.cpp \ + includes/jetracer/pid_controller.hpp \ + /usr/include/opencv4/opencv2/opencv.hpp \ + /usr/include/opencv4/opencv2/opencv_modules.hpp \ + /usr/include/opencv4/opencv2/core.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/core/version.hpp \ + /usr/include/opencv4/opencv2/core/hal/interface.h \ + /usr/include/opencv4/opencv2/core/cv_cpu_dispatch.h \ + /usr/include/opencv4/opencv2/core/base.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.hpp \ + /usr/include/opencv4/opencv2/core/cvstd_wrapper.hpp \ + /usr/include/opencv4/opencv2/core/neon_utils.hpp \ + /usr/include/opencv4/opencv2/core/vsx_utils.hpp \ + /usr/include/opencv4/opencv2/core/check.hpp \ + /usr/include/opencv4/opencv2/core/traits.hpp \ + /usr/include/opencv4/opencv2/core/matx.hpp \ + /usr/include/opencv4/opencv2/core/saturate.hpp \ + /usr/include/opencv4/opencv2/core/fast_math.hpp \ + /usr/include/opencv4/opencv2/core/types.hpp \ + /usr/include/opencv4/opencv2/core/mat.hpp \ + /usr/include/opencv4/opencv2/core/bufferpool.hpp \ + /usr/include/opencv4/opencv2/core/mat.inl.hpp \ + /usr/include/opencv4/opencv2/core/persistence.hpp \ + /usr/include/opencv4/opencv2/core/operations.hpp \ + /usr/include/opencv4/opencv2/core/cvstd.inl.hpp \ + /usr/include/opencv4/opencv2/core/utility.hpp \ + /usr/include/opencv4/opencv2/core/optim.hpp \ + /usr/include/opencv4/opencv2/core/ovx.hpp \ + /usr/include/opencv4/opencv2/core/cvdef.h \ + /usr/include/opencv4/opencv2/calib3d.hpp \ + /usr/include/opencv4/opencv2/features2d.hpp \ + /usr/include/opencv4/opencv2/flann/miniflann.hpp \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/config.h \ + /usr/include/opencv4/opencv2/core/affine.hpp \ + /usr/include/opencv4/opencv2/dnn.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.hpp \ + /usr/include/opencv4/opencv2/core/async.hpp \ + /usr/include/opencv4/opencv2/dnn/../dnn/version.hpp \ + /usr/include/opencv4/opencv2/dnn/dict.hpp \ + /usr/include/opencv4/opencv2/dnn/layer.hpp \ + /usr/include/opencv4/opencv2/dnn/dnn.inl.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/inference_engine.hpp \ + /usr/include/opencv4/opencv2/dnn/utils/../dnn.hpp \ + /usr/include/opencv4/opencv2/flann.hpp \ + /usr/include/opencv4/opencv2/flann/flann_base.hpp \ + /usr/include/opencv4/opencv2/flann/general.h \ + /usr/include/opencv4/opencv2/flann/matrix.h \ + /usr/include/opencv4/opencv2/flann/params.h \ + /usr/include/opencv4/opencv2/flann/any.h \ + /usr/include/opencv4/opencv2/flann/defines.h \ + /usr/include/opencv4/opencv2/flann/saving.h \ + /usr/include/opencv4/opencv2/flann/nn_index.h \ + /usr/include/opencv4/opencv2/flann/result_set.h \ + /usr/include/opencv4/opencv2/flann/all_indices.h \ + /usr/include/opencv4/opencv2/flann/kdtree_index.h \ + /usr/include/opencv4/opencv2/flann/dynamic_bitset.h \ + /usr/include/opencv4/opencv2/flann/dist.h \ + /usr/include/opencv4/opencv2/flann/heap.h \ + /usr/include/opencv4/opencv2/flann/allocator.h \ + /usr/include/opencv4/opencv2/flann/random.h \ + /usr/include/opencv4/opencv2/flann/kdtree_single_index.h \ + /usr/include/opencv4/opencv2/flann/kmeans_index.h \ + /usr/include/opencv4/opencv2/flann/logger.h \ + /usr/include/opencv4/opencv2/flann/composite_index.h \ + /usr/include/opencv4/opencv2/flann/linear_index.h \ + /usr/include/opencv4/opencv2/flann/hierarchical_clustering_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_index.h \ + /usr/include/opencv4/opencv2/flann/lsh_table.h \ + /usr/include/opencv4/opencv2/flann/autotuned_index.h \ + /usr/include/opencv4/opencv2/flann/ground_truth.h \ + /usr/include/opencv4/opencv2/flann/index_testing.h \ + /usr/include/opencv4/opencv2/flann/timer.h \ + /usr/include/opencv4/opencv2/flann/sampling.h \ + /usr/include/opencv4/opencv2/highgui.hpp \ + /usr/include/opencv4/opencv2/imgcodecs.hpp \ + /usr/include/opencv4/opencv2/videoio.hpp \ + /usr/include/opencv4/opencv2/imgproc.hpp \ + /usr/include/opencv4/opencv2/./imgproc/segmentation.hpp \ + /usr/include/opencv4/opencv2/ml.hpp \ + /usr/include/opencv4/opencv2/ml/ml.inl.hpp \ + /usr/include/opencv4/opencv2/objdetect.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_dictionary.hpp \ + /usr/include/opencv4/opencv2/objdetect/aruco_board.hpp \ + /usr/include/opencv4/opencv2/objdetect/graphical_code_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/detection_based_tracker.hpp \ + /usr/include/opencv4/opencv2/objdetect/face.hpp \ + /usr/include/opencv4/opencv2/objdetect/charuco_detector.hpp \ + /usr/include/opencv4/opencv2/objdetect/barcode.hpp \ + /usr/include/opencv4/opencv2/photo.hpp \ + /usr/include/opencv4/opencv2/stitching.hpp \ + /usr/include/opencv4/opencv2/stitching/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/core/cuda.hpp \ + /usr/include/opencv4/opencv2/core/cuda_types.hpp \ + /usr/include/opencv4/opencv2/core/cuda.inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/warpers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/matchers.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/util_inl.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/exposure_compensate.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/seam_finders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/blenders.hpp \ + /usr/include/opencv4/opencv2/stitching/detail/camera.hpp \ + /usr/include/opencv4/opencv2/video.hpp \ + /usr/include/opencv4/opencv2/video/tracking.hpp \ + /usr/include/opencv4/opencv2/video/background_segm.hpp \ + includes/jetracer/computer_vision.hpp diff --git a/emergency_stop/build/pid_controller.o b/emergency_stop/build/pid_controller.o new file mode 100644 index 000000000..8a900af92 Binary files /dev/null and b/emergency_stop/build/pid_controller.o differ diff --git a/emergency_stop/docs/emergency_stop_system.md b/emergency_stop/docs/emergency_stop_system.md new file mode 100644 index 000000000..0eecd2887 --- /dev/null +++ b/emergency_stop/docs/emergency_stop_system.md @@ -0,0 +1,98 @@ +# Emergency Stop System + +## Overview + +The emergency stop system has been implemented to automatically detect obstacles in the danger zone and trigger an immediate stop when occupancy exceeds 25% of the area. + +## Implemented Components + +### 1. Occupancy Calculation Function (`calculateDangerZoneOccupancy`) + +**Location:** `sources/computer_vision.cpp` + +**Functionality:** +- Calculates the percentage of area occupied by obstacles in the danger zone +- Creates a danger zone mask based on detected lane curves +- Identifies white pixels (obstacles) within the danger zone +- Returns an occupancy percentage (0.0 to 1.0) + +**Parameters:** +- `mask`: Binary image mask +- `left_curve`: Left lane points +- `right_curve`: Right lane points +- `displacement_cm`: Lane width in centimeters +- `scale`: Pixel/cm conversion scale + +### 2. Emergency Stop Function (`emergency_stop`) + +**Location:** `sources/jetracer.cpp` + +**Functionality:** + +- Maintains speed 0 until the danger zone is clear +- Centers the steering +- Updates all state variables +- Stops speed tests if active +- Displays detailed log messages + +### 3. Integration in Main Loop + +**Location:** `apps/main.cpp` + +**Functionality:** +- Detects lane curves in real-time +- Calculates danger zone occupancy each frame +- Triggers emergency stop if occupancy > 25% +- Waits 500ms after stop before continuing (optimized) + +### 4. Visual Indicators in Stream + +**Functionality:** +- Shows danger zone occupancy percentage +- Dynamic colors: + - Green: < 15% (safe) + - Yellow: 15-25% (attention) + - Red: > 25% (emergency) +- Displays "EMERGENCY STOP!" when activated + +## Operation Flow + +``` +1. Camera frame capture +2. Binary mask processing +3. Lane curve detection +4. Danger zone calculation +5. Obstacle identification in zone +6. Occupancy percentage calculation +7. Threshold verification (25%) +8. If exceeded: Trigger emergency stop +9. Update visual indicators +10. Continue to next frame +``` + +## Configuration + +### Emergency Threshold +- **Value:** 25% (0.25) +- **Location:** `apps/main.cpp` - constant `EMERGENCY_THRESHOLD` +- **Adjustable:** Yes, modify the constant + +### Visual Color Thresholds +- **Green:** < 15% (safe) +- **Yellow:** 15-25% (attention) +- **Red:** > 25% (emergency) + +## Safety + +### Safety Features: + +4. **Centering:** Steering is automatically centered +5. **Clean State:** All variables are reset +6. **Detailed Logs:** Complete action recording +7. **Recovery Time:** 500ms pause after stop (optimized) + +### False Positive Prevention: +- Uses robust lane detection +- Considers only the lower half of the image +- Applies morphological filters to reduce noise +- Requires significant occupancy (25%) to activate diff --git a/emergency_stop/includes/jetracer/computer_vision.hpp b/emergency_stop/includes/jetracer/computer_vision.hpp new file mode 100644 index 000000000..b912fa0c1 --- /dev/null +++ b/emergency_stop/includes/jetracer/computer_vision.hpp @@ -0,0 +1,79 @@ +#ifndef COMPUTER_VISION_HPP +#define COMPUTER_VISION_HPP + +#include +#include +#include + +namespace jetracer::vision +{ + // Constants for image dimensions and shared memory + constexpr int WIDTH = 128; + constexpr int HEIGHT = 128; + constexpr int SIZE = WIDTH * HEIGHT; + + float getXAtY(float y, float y0, float x0, float vx, float vy); + + bool extractLanePoints(const cv::Mat &frame, + float image_center, + float &y_ref, + std::vector &left_point, + std::vector &right_point); + + float calculateTrackCenter(const std::vector &left, + const std::vector &right, + float y_ref, + float displacement_cm, + float scale, + cv::Mat &frame); + + void draw_overlay(cv::Mat &frame, + float erro, + float pid, + const std::string &file_name, + const std::string &txt_lane, + float image_center, + float center_track, + float y_ref); + + // Functions for danger zone detection and display + bool sampleLaneEdgesByRow(const cv::Mat &mask, + int y_start, int y_end, int step, + float image_center, + std::vector &left_curve, + std::vector &right_curve, + int min_run = 3); + + void drawDangerZoneCurved(cv::Mat &frame, + const std::vector &left_curve, + const std::vector &right_curve, + float displacement_cm, float scale); + + float calculateDangerZoneOccupancy(const cv::Mat &mask, + const std::vector &left_curve, + const std::vector &right_curve, + float displacement_cm, + float scale); + + void createDangerZoneMask(const cv::Mat &original_mask, + const std::vector &left_curve, + const std::vector &right_curve, + float displacement_cm, float scale, + cv::Mat &danger_zone_mask); + + float calculateDangerZoneOccupancyFromMask(const cv::Mat &original_mask, + const cv::Mat &danger_zone_mask); + + // Functions for drivable area danger zone + void createDrivableDangerZoneMask(const cv::Mat &drivable_mask, + float displacement_cm, float scale, + cv::Mat &drivable_danger_zone_mask); + + float calculateDrivableDangerZoneOccupancy(const cv::Mat &drivable_mask, + const cv::Mat &drivable_danger_zone_mask); + + void drawDrivableDangerZone(cv::Mat &frame, + const cv::Mat &drivable_danger_zone_mask); +} // namespace jetracer::vision + +#endif // COMPUTER_VISION_HPP diff --git a/pid_control/includes/jetracer/i2c_device.hpp b/emergency_stop/includes/jetracer/i2c_device.hpp similarity index 99% rename from pid_control/includes/jetracer/i2c_device.hpp rename to emergency_stop/includes/jetracer/i2c_device.hpp index a20e12a73..d38d3366d 100644 --- a/pid_control/includes/jetracer/i2c_device.hpp +++ b/emergency_stop/includes/jetracer/i2c_device.hpp @@ -14,6 +14,7 @@ namespace jetracer::hardware void write_byte(uint8_t reg, uint8_t value); uint8_t read_byte(uint8_t reg); + private: int fd_; }; diff --git a/emergency_stop/includes/jetracer/jetracer.hpp b/emergency_stop/includes/jetracer/jetracer.hpp new file mode 100644 index 000000000..75d875ee0 --- /dev/null +++ b/emergency_stop/includes/jetracer/jetracer.hpp @@ -0,0 +1,117 @@ +#ifndef JETRACER_HPP +#define JETRACER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "jetracer/i2c_device.hpp" +#include "jetracer/pwm_config.hpp" +#include "jetracer/motor_control.hpp" + +namespace jetracer::control +{ + class JetRacer + { + public: + JetRacer(int servo_addr, int motor_addr); + ~JetRacer(); + + void start(); + void stop(); + bool is_running() const; + void set_speed(float speed); + void set_steering(int angle); + void smooth_steering(int target_angle, int increment); + + void set_constant_speed_mode(bool enabled); + void set_test_speed(float speed_percent); + void set_test_duration(int seconds); + void start_speed_test(); + void stop_speed_test(); + bool is_test_mode() const { return test_mode_; } + + void emergency_stop(); + + void release_motor_lock(); + + bool is_emergency_braking_active(); + + void set_cruise_control_mode(bool enabled); + void set_cruise_control_speed(float speed); + bool is_cruise_control_active() const { return cruise_control_active_; } + float get_cruise_control_speed() const { return cruise_control_speed_; } + + int servo_delay_ms_ = 30; + + static constexpr int PWM_FREQUENCY_HZ = pwm::frequency::MOTOR_MED_FREQ; // Default PWM frequency for DC motors + static constexpr int SPEED_SMOOTHING_WINDOW = pwm::smoothing::BALANCED; // Window for speed smoothing + static constexpr float MAX_SPEED_CHANGE_PER_UPDATE = pwm::smoothing::MEDIUM_CHANGE; // Maximum speed change per update + + private: + void init_servo(); + void init_motors(); + void set_servo_pwm(int channel, int on_value, int off_value); + void set_motor_pwm(int channel, int value); + void set_motor_pwm_smooth(int channel, int value); // PWM with smoothing + void process_joystick(); + void process_test_mode(); + float smooth_speed(float target_speed); + float calculate_safe_speed(float target_speed); + + static constexpr int MAX_ANGLE_ = 140; + static constexpr int SERVO_LEFT_PWM_ = 140; + static constexpr int SERVO_CENTER_PWM_ = 280; + static constexpr int SERVO_RIGHT_PWM_ = 420; + + int servo_addr_; + int motor_addr_; + std::atomic running_; + hardware::I2CDevice servo_device_; + hardware::I2CDevice motor_device_; + int current_angle_ = 0; + float current_speed_ = 0.0f; + + // Variables for speed smoothing + std::deque speed_history_; + float smoothed_speed_ = 0.0f; + + // Variables for advanced motor control + float filtered_speed_ = 0.0f; + float target_speed_ = 0.0f; + float last_speed_command_ = 0.0f; + unsigned long last_movement_time_ = 0; // Timestamp of last movement command + + // Variables for constant speed test mode + std::atomic test_mode_{false}; + std::atomic test_speed_{0.0f}; + std::atomic test_duration_{0}; + std::atomic test_running_{false}; + std::chrono::steady_clock::time_point test_start_time_; + std::thread test_thread_; + + // Variables for cruise control (autonomous mode) + std::atomic cruise_control_active_{false}; + std::atomic cruise_control_speed_{0.0f}; + std::atomic r2_button_pressed_{false}; + std::atomic r2_button_was_pressed_{false}; + + // Variable to control motor lock during emergency stop + std::atomic motor_locked_{false}; + + // Variables to control emergency braking for 3 seconds + std::atomic emergency_braking_active_{false}; + std::chrono::steady_clock::time_point emergency_brake_start_time_; + static constexpr int EMERGENCY_BRAKE_DURATION_MS = 3000; // 3 seconds + }; +} // namespace jetracer::control + +#endif // JETRACER_HPP diff --git a/emergency_stop/includes/jetracer/motor_control.hpp b/emergency_stop/includes/jetracer/motor_control.hpp new file mode 100644 index 000000000..7f992aab9 --- /dev/null +++ b/emergency_stop/includes/jetracer/motor_control.hpp @@ -0,0 +1,44 @@ +#ifndef MOTOR_CONTROL_HPP +#define MOTOR_CONTROL_HPP + +namespace jetracer::motor_control +{ + // Deadzone and threshold configurations + namespace thresholds + { + static constexpr float SPEED_DEADZONE = 0.001f; // 0.1% - Minimum deadzone for maximum responsiveness + static constexpr int MIN_PWM_THRESHOLD = 500; // Reduced minimum PWM for slower car (12%) + static constexpr int TORQUE_BOOST_THRESHOLD = 1200; // Reduced threshold for less amplification + static constexpr float LOW_SPEED_AMPLIFICATION = 1.8f; // Reduced amplification for slower car + } + + // Power curve configurations + namespace power + { + static constexpr float POWER_CURVE_FACTOR = 0.7f; // Exponential power curve factor (reduced for slower car) + static constexpr float SPEED_SMOOTHING_FACTOR = 0.88f; // Speed smoothing factor (reduced) + } + + // Acceleration ramp configurations + namespace ramps + { + static constexpr float ACCELERATION_RAMP = 8.0f; // Much increased acceleration rate (% per update) + static constexpr float DECELERATION_RAMP = 15.0f; // Much increased deceleration rate (% per update) + static constexpr float EMERGENCY_BRAKE_THRESHOLD = 50.0f; // Threshold for emergency braking + } + + // Safety configurations + namespace safety + { + static constexpr float DIRECTION_CHANGE_THRESHOLD = 10.0f; // Threshold to detect direction change + static constexpr float MAX_SPEED_PERCENT = 100.0f; // Maximum allowed speed (without artificial limitation) + + // Speed limitations for curve safety + static constexpr float BASE_MAX_SPEED = 40.0f; + static constexpr float CURVE_SPEED_REDUCTION = 1.1f; // Speed boost in curves (110% of base speed) - reduced + static constexpr float STRAIGHT_SPEED_BOOST = 1.05f; // No boost in straights (100% of base speed) + static constexpr float STEERING_ANGLE_THRESHOLD = 25.0f; // Steering angle to consider as "curve" - increased to be less sensitive + } +} + +#endif // MOTOR_CONTROL_HPP diff --git a/emergency_stop/includes/jetracer/pid_controller.hpp b/emergency_stop/includes/jetracer/pid_controller.hpp new file mode 100644 index 000000000..a2655f560 --- /dev/null +++ b/emergency_stop/includes/jetracer/pid_controller.hpp @@ -0,0 +1,18 @@ +#ifndef PID_CONTROLLER_HPP +#define PID_CONTROLLER_HPP + +#include +#include + +namespace jetracer::pid +{ + struct PIDStatus + { + float integral_error = 0.0f; + float previous_error = 0.0f; + }; + float PIDapply(float error, float dt, PIDStatus &status); + float PIDexecute(const cv::Mat &original_frame); +} // namespace jetracer::pid + +#endif // PID_CONTROLLER_HPP diff --git a/emergency_stop/includes/jetracer/pwm_config.hpp b/emergency_stop/includes/jetracer/pwm_config.hpp new file mode 100644 index 000000000..3afab80b5 --- /dev/null +++ b/emergency_stop/includes/jetracer/pwm_config.hpp @@ -0,0 +1,49 @@ +#ifndef PWM_CONFIG_HPP +#define PWM_CONFIG_HPP + +namespace jetracer::pwm +{ + // PWM frequency configurations + namespace frequency + { + // Recommended frequencies for different applications + static constexpr int MOTOR_LOW_FREQ = 500; // 500 Hz - For low power motors + static constexpr int MOTOR_MED_FREQ = 1000; // 1000 Hz - Standard for DC motors + static constexpr int MOTOR_HIGH_FREQ = 2000; // 2000 Hz - For high precision motors + static constexpr int SERVO_FREQ = 50; // 50 Hz - Standard for servos + } + + // Smoothing configurations + namespace smoothing + { + // Smoothing windows for different types of movement + static constexpr int AGGRESSIVE = 3; // Fast response, less smooth + static constexpr int BALANCED = 5; // Balance between smoothness and response + static constexpr int SMOOTH = 7; // Very smooth, slower response + static constexpr int ULTRA_SMOOTH = 10; // Extremely smooth + + // Speed change limits per update + static constexpr float FAST_CHANGE = 3.0f; // Fast change (reduced) + static constexpr float MEDIUM_CHANGE = 1.5f; // Medium change (default, reduced) + static constexpr float SLOW_CHANGE = 0.8f; // Slow change (reduced) + static constexpr float ULTRA_SLOW_CHANGE = 0.3f; // Very slow change (reduced) + } + + // Timing configurations + namespace timing + { + // Control loop update frequencies + static constexpr int JOYSTICK_UPDATE_MS = 25; // 40 Hz - Joystick (more time to process frames) + static constexpr int PID_UPDATE_MS = 30; // 33 Hz - PID (more time to process) + static constexpr int MOTOR_UPDATE_MS = 10; // 100 Hz - Motors (reduced to give more time) + } + + // Safety configurations + namespace safety + { + static constexpr float MAX_SPEED_PERCENT = 46.0f; + static constexpr float EMERGENCY_STOP_DELAY_MS = 150.0f; // Delay for emergency stop (increased) + } +} + +#endif // PWM_CONFIG_HPP diff --git a/emergency_stop/models/best_202507181755.pt b/emergency_stop/models/best_202507181755.pt new file mode 100644 index 000000000..66d963056 Binary files /dev/null and b/emergency_stop/models/best_202507181755.pt differ diff --git a/emergency_stop/scripts/camera_yolo_to_shm.py b/emergency_stop/scripts/camera_yolo_to_shm.py new file mode 100644 index 000000000..4c6f028d3 --- /dev/null +++ b/emergency_stop/scripts/camera_yolo_to_shm.py @@ -0,0 +1,116 @@ +# File: camera_yolo_to_shm.py +import cv2 +import numpy as np +import time +import os +from ultralytics import YOLO +from multiprocessing import shared_memory + +# ===== Configurations ===== +IMG_WIDTH = 128 +IMG_HEIGHT = 128 +SHM_NAME = "mask_shared" +MODEL_PATH = "models/best_202507181755.pt" +LANE_CLASS_ID = 1 +DRIVABLE_CLASS_ID = 0 +CONF_THRESHOLD = 0.25 +SAVE_DIR = "masks" + +# ===== CSI camera GStreamer pipeline ===== +PIPELINE = ( + "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=320, height=240, format=NV12, framerate=15/1 ! " + "nvvidconv ! video/x-raw, format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink drop=true max-buffers=1" +) + +# ===== Initialize shared memory (flag + 2 images) ===== +TOTAL_SIZE = 1 + 2 * IMG_WIDTH * IMG_HEIGHT # 1 byte for flag + 2 masks +try: + shm = shared_memory.SharedMemory(name=SHM_NAME, create=True, size=TOTAL_SIZE) +except FileExistsError: + existing = shared_memory.SharedMemory(name=SHM_NAME) + existing.close() + existing.unlink() + shm = shared_memory.SharedMemory(name=SHM_NAME, create=True, size=TOTAL_SIZE) + +flag_buf = np.ndarray((1,), dtype=np.uint8, buffer=shm.buf, offset=0) +lane_mask_buf = np.ndarray((IMG_HEIGHT, IMG_WIDTH), dtype=np.uint8, buffer=shm.buf, offset=1) +drivable_mask_buf = np.ndarray((IMG_HEIGHT, IMG_WIDTH), dtype=np.uint8, buffer=shm.buf, offset=1 + IMG_WIDTH * IMG_HEIGHT) + +# ===== Load YOLO model and camera ===== +model = YOLO(MODEL_PATH) +cap = cv2.VideoCapture(PIPELINE, cv2.CAP_GSTREAMER) + +#for _ in range(5): +# cap.read() +# time.sleep(0.05) + +if not cap.isOpened(): + print("Error opening CSI camera.") + shm.close() + shm.unlink() + exit(1) + +os.makedirs(SAVE_DIR, exist_ok=True) +print("Camera and model loaded. Press ESC to exit.") + +try: + while True: + start = time.time() + + ret, frame = cap.read() + if not ret: + print("Frame not captured.") + break + + results = model.predict(source=frame, conf=CONF_THRESHOLD, verbose=False) + lane_mask_final = np.zeros((frame.shape[0], frame.shape[1]), dtype=np.uint8) + drivable_mask_final = np.zeros((frame.shape[0], frame.shape[1]), dtype=np.uint8) + + if results[0].masks is not None and results[0].boxes is not None: + masks = results[0].masks.data.cpu().numpy() + classes = results[0].boxes.cls.cpu().numpy().astype(int) + + for i, cls_id in enumerate(classes): + try: + mask_i = cv2.resize(masks[i], (frame.shape[1], frame.shape[0])) + mask_i = (mask_i > 0.5).astype(np.uint8) + + if cls_id == LANE_CLASS_ID: + lane_mask_final = np.logical_or(lane_mask_final, mask_i) + elif cls_id == DRIVABLE_CLASS_ID: + drivable_mask_final = np.logical_or(drivable_mask_final, mask_i) + + except Exception as e: + print(f"Error processing mask {i}: {e}") + + lane_mask_final = (lane_mask_final * 255).astype(np.uint8) + drivable_mask_final = (drivable_mask_final * 255).astype(np.uint8) + + lane_mask_resized = cv2.resize(lane_mask_final, (IMG_WIDTH, IMG_HEIGHT)) + drivable_mask_resized = cv2.resize(drivable_mask_final, (IMG_WIDTH, IMG_HEIGHT)) + + # === Synchronization: wait for C++ to process (flag == 0) === + while flag_buf[0] != 0: + time.sleep(0.001) + + # === Send masks and signal (flag = 1) === + lane_mask_buf[:] = lane_mask_resized[:] + drivable_mask_buf[:] = drivable_mask_resized[:] + flag_buf[0] = 1 + + fps = 1 / (time.time() - start) + print(f"FPS: {fps:.2f}") + + #cv2.imshow("CSI Camera", frame) + #cv2.imshow("Lane Mask", lane_mask_resized) + #cv2.imshow("Drivable Mask", drivable_mask_resized) + + if cv2.waitKey(1) == 27: + break + +finally: + cap.release() + shm.close() + shm.unlink() + cv2.destroyAllWindows() + print("Closed.") diff --git a/emergency_stop/scripts/extract_info_yolo_model.py b/emergency_stop/scripts/extract_info_yolo_model.py new file mode 100644 index 000000000..30396eb6d --- /dev/null +++ b/emergency_stop/scripts/extract_info_yolo_model.py @@ -0,0 +1,50 @@ +from ultralytics import YOLO +import torch +import os + +# Model path +model_path = '../models/best_202507181755.pt' + +# Check if file exists +if not os.path.exists(model_path): + print(f"[ERROR] File '{model_path}' not found.") + exit(1) + +print(f"\n[INFO] Loading model: {model_path}") +model = YOLO(model_path) + +# Basic information +print("\nBasic model information:") +print(f"- Model type: {type(model)}") +print(f"- Number of classes: {model.model.nc}") +print(f"- Class names (model.names): {model.names}") + +# Model structure +print("\nModel structure (summary):") +model.info(verbose=True) + +# Training arguments and hyperparameters +print("\nTraining arguments and hyperparameters available in the model:") +try: + args = model.model.args + for k, v in vars(args).items(): + print(f" - {k}: {v}") +except Exception as e: + print(" [!] Could not access 'args' from the Ultralytics model.") + +# Extra: inspect using PyTorch +print("\nDirect inspection using PyTorch:") +try: + raw_model = torch.load(model_path, map_location='cpu') + print("Keys found in the PyTorch dictionary:") + print(list(raw_model.keys())) + + if 'train_args' in raw_model: + print("\nTraining arguments ('train_args') found:") + for k, v in raw_model['train_args'].items(): + print(f" - {k}: {v}") + else: + print(" [!] No 'train_args' found in the model.") + +except Exception as e: + print(f"[ERROR] Failed to load model with PyTorch: {e}") diff --git a/emergency_stop/sources/computer_vision.cpp b/emergency_stop/sources/computer_vision.cpp new file mode 100644 index 000000000..2524db0d1 --- /dev/null +++ b/emergency_stop/sources/computer_vision.cpp @@ -0,0 +1,557 @@ +#include "jetracer/computer_vision.hpp" +#include + +namespace jetracer::vision +{ + constexpr int roi_numbers_in_frame = 7; + + float getXAtY(float y, float y0, float x0, float vx, float vy) + { + return x0 + (y - y0) * (vx / vy); + } + + void draw_overlay(cv::Mat &frame, float erro, float pid, const std::string &file_name, const std::string &txt_lane, float image_center, float center_track, float y_ref) + { + cv::line(frame, {int(image_center), int(y_ref)}, {int(image_center), frame.rows}, {0, 150, 0}, 2); + cv::line(frame, {int(center_track), int(y_ref)}, {int(center_track), frame.rows}, {200, 200, 200}, 2); + cv::line(frame, {0, int(y_ref)}, {frame.cols, int(y_ref)}, {255, 255, 255}, 1); + cv::circle(frame, {int(center_track), int(y_ref)}, 5, {255, 0, 0}, -1); + + char buffer[100]; + std::snprintf(buffer, sizeof(buffer), "Lateral error: %.2f deg", erro); + std::string txt_erro(buffer); + std::snprintf(buffer, sizeof(buffer), "PID correction: %.2f deg", pid); + std::string txt_pid(buffer); + + cv::putText(frame, file_name, {10, 30}, cv::FONT_HERSHEY_SIMPLEX, 0.7, {255, 255, 255}, 2); + cv::putText(frame, txt_erro, {10, 55}, cv::FONT_HERSHEY_SIMPLEX, 0.6, {255, 255, 255}, 1); + cv::putText(frame, txt_pid, {10, 75}, cv::FONT_HERSHEY_SIMPLEX, 0.6, {255, 255, 255}, 1); + cv::putText(frame, txt_lane, {10, 95}, cv::FONT_HERSHEY_SIMPLEX, 0.6, {255, 255, 255}, 1); + } + + bool extractLanePoints(const cv::Mat &frame, float image_center, float &y_ref, std::vector &left_point, std::vector &right_point) + { + int height = frame.rows; + + for (int i = 3; i < roi_numbers_in_frame; ++i) + { + int roi_y = (height * i) / roi_numbers_in_frame; + int roi_height = (height * (i + 1)) / roi_numbers_in_frame - roi_y; + y_ref = roi_y + roi_height / 2; + + cv::Mat roi = frame(cv::Rect(0, roi_y, frame.cols, roi_height)).clone(); + std::vector> contours; + cv::findContours(roi, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + float best_left = std::numeric_limits::max(); + float best_right = std::numeric_limits::max(); + cv::Point2f center_left, center_right; + + for (const auto &c : contours) + { + if (c.size() < 5) + continue; + + cv::Vec4f line; + cv::fitLine(c, line, cv::DIST_L2, 0, 0.01, 0.01); + float vx = line[0], x0 = line[2]; + float slope = line[1] / (vx + 1e-5); + float dist = std::abs(x0 - image_center); + + if (slope < -0.3f && dist < best_left) + { + best_left = dist; + center_left = {x0, line[3] + roi_y}; + } + else if (slope > 0.3f && dist < best_right) + { + best_right = dist; + center_right = {x0, line[3] + roi_y}; + } + } + + if (best_left < std::numeric_limits::max()) + left_point = {{int(center_left.x), int(center_left.y)}, {int(center_left.x), int(center_left.y + 5)}}; + + if (best_right < std::numeric_limits::max()) + right_point = {{int(center_right.x), int(center_right.y)}, {int(center_right.x), int(center_right.y + 5)}}; + + if (!left_point.empty() || !right_point.empty()) + return true; + } + return false; + } + + float calculateTrackCenter(const std::vector &left, const std::vector &right, float y_ref, float displacement_cm, float scale, cv::Mat &frame) + { + if (!left.empty() && !right.empty()) + { + cv::Vec4f l1, l2; + cv::fitLine(left, l1, cv::DIST_L2, 0, 0.01, 0.01); + cv::fitLine(right, l2, cv::DIST_L2, 0, 0.01, 0.01); + float x_left = getXAtY(y_ref, l1[3], l1[2], l1[0], l1[1]); + float x_right = getXAtY(y_ref, l2[3], l2[2], l2[0], l2[1]); + cv::circle(frame, {int(x_left), int(y_ref)}, 4, {200, 255, 200}, -1); + cv::circle(frame, {int(x_right), int(y_ref)}, 4, {200, 100, 255}, -1); + return (x_left + x_right) / 2.0f; + } + if (!right.empty()) + { + cv::Vec4f l2; + cv::fitLine(right, l2, cv::DIST_L2, 0, 0.01, 0.01); + float x_right = getXAtY(y_ref, l2[3], l2[2], l2[0], l2[1]); + cv::circle(frame, {int(x_right), int(y_ref)}, 4, {200, 100, 255}, -1); + return x_right - (displacement_cm / scale); + } + if (!left.empty()) + { + cv::Vec4f l1; + cv::fitLine(left, l1, cv::DIST_L2, 0, 0.01, 0.01); + float x_left = getXAtY(y_ref, l1[3], l1[2], l1[0], l1[1]); + cv::circle(frame, {int(x_left), int(y_ref)}, 4, {200, 255, 200}, -1); + return x_left + (displacement_cm / scale); + } + return -1.0f; + } + + float calculateDangerZoneOccupancy(const cv::Mat &mask, + const std::vector &left_curve, + const std::vector &right_curve, + float displacement_cm, + float scale) + { + // Mark parameters as intentionally unused (for future compatibility) + (void)displacement_cm; + (void)scale; + + if (mask.empty() || left_curve.empty() || right_curve.empty()) + { + return 0.0f; + } + + // Create danger zone mask + cv::Mat danger_zone_mask = cv::Mat::zeros(mask.size(), CV_8UC1); + + // Define the danger zone as the area between lanes in the lower half of the image + int height = mask.rows; + int start_y = height / 2; // Start from the middle of the image + + // Create points for the danger zone + std::vector danger_zone_points; + + // Add points from left lane (from middle down) + for (const auto &point : left_curve) + { + if (point.y >= start_y) + { + danger_zone_points.push_back(point); + } + } + + // Add points from right lane (from middle down) + for (const auto &point : right_curve) + { + if (point.y >= start_y) + { + danger_zone_points.push_back(point); + } + } + + // If we don't have enough points, return 0 + if (danger_zone_points.size() < 4) + { + return 0.0f; + } + + // Create danger zone polygon + cv::fillPoly(danger_zone_mask, std::vector>{danger_zone_points}, cv::Scalar(255)); + + // Calculate total area of danger zone + int total_danger_area = cv::countNonZero(danger_zone_mask); + if (total_danger_area == 0) + { + return 0.0f; + } + + // Apply danger zone mask to original mask + cv::Mat masked_obstacles; + cv::bitwise_and(mask, danger_zone_mask, masked_obstacles); + + // Count white pixels (obstacles) in danger zone + int obstacle_pixels = cv::countNonZero(masked_obstacles); + + // Calculate occupancy percentage + float occupancy = static_cast(obstacle_pixels) / static_cast(total_danger_area); + + return occupancy; + } + + // Scans the lower half of the mask and extracts lane curves. + // min_run helps ignore noise by requiring at least N contiguous white pixels. + bool sampleLaneEdgesByRow(const cv::Mat &mask, + int y_start, int y_end, int step, + float image_center, + std::vector &left_curve, + std::vector &right_curve, + int min_run) + { + CV_Assert(mask.type() == CV_8UC1); + y_start = std::max(0, y_start); + y_end = std::min(mask.rows, y_end); + left_curve.clear(); + right_curve.clear(); + + const int width = mask.cols; + bool any = false; + + for (int y = y_start; y < y_end; y += step) + { + const uchar *row = mask.ptr(y); + + // --- search left lane (left half; from center to left) --- + int run = 0, sumx = 0; + int x_left = -1; + for (int x = int(image_center) - 1; x >= 0; --x) + { + if (row[x] > 0) + { + run++; + sumx += x; + } + else + { + if (run >= min_run) + { + x_left = sumx / run; + break; + } + run = 0; + sumx = 0; + } + } + if (x_left == -1 && run >= min_run) + x_left = sumx / run; + + // --- search right lane (right half; from center to right) --- + run = 0; + sumx = 0; + int x_right = -1; + for (int x = int(image_center); x < width; ++x) + { + if (row[x] > 0) + { + run++; + sumx += x; + } + else + { + if (run >= min_run) + { + x_right = sumx / run; + break; + } + run = 0; + sumx = 0; + } + } + if (x_right == -1 && run >= min_run) + x_right = sumx / run; + + if (x_left >= 0) + { + left_curve.emplace_back(x_left, y); + any = true; + } + if (x_right >= 0) + { + right_curve.emplace_back(x_right, y); + any = true; + } + } + return any && (!left_curve.empty() || !right_curve.empty()); + } + + // Draws the curved danger zone between the two curves. + // If only one lane exists, estimates the other by offsetting by lane_width_pixels. + void drawDangerZoneCurved(cv::Mat &frame, + const std::vector &left_curve, + const std::vector &right_curve, + float displacement_cm, float scale) + { + if (frame.empty()) + return; + + const int start_y = frame.rows * 2 / 5; // Lower 3/5 of frame + const int width = frame.cols; + const int end_y = frame.rows - 1; + const int min_pts = 8; // avoids degenerate polygons + const int lane_width_pixels = std::max(1, int(displacement_cm / scale)); + + std::vector poly; + + if (!left_curve.empty() && !right_curve.empty()) + { + // left: from bottom to top (large y -> small) + for (auto it = left_curve.rbegin(); it != left_curve.rend(); ++it) + if (it->y >= start_y && it->y <= end_y) + poly.push_back(*it); + + // right: from top to bottom (small y -> large) + for (const auto &p : right_curve) + if (p.y >= start_y && p.y <= end_y) + poly.push_back(p); + } + else if (!right_curve.empty()) + { + // only right → estimate left by offset + for (auto it = right_curve.rbegin(); it != right_curve.rend(); ++it) + { + if (it->y >= start_y && it->y <= end_y) + { + int xl = std::max(0, it->x - lane_width_pixels); + poly.emplace_back(xl, it->y); + } + } + for (const auto &p : right_curve) + if (p.y >= start_y && p.y <= end_y) + poly.push_back(p); + } + else if (!left_curve.empty()) + { + // only left → estimate right by offset + for (const auto &p : left_curve) + if (p.y >= start_y && p.y <= end_y) + poly.push_back(p); + + for (auto it = left_curve.rbegin(); it != left_curve.rend(); ++it) + { + if (it->y >= start_y && it->y <= end_y) + { + int xr = std::min(width - 1, it->x + lane_width_pixels); + poly.emplace_back(xr, it->y); + } + } + } + + if ((int)poly.size() >= min_pts) + { + if (frame.channels() == 1) + cv::fillPoly(frame, std::vector>{poly}, cv::Scalar(255)); + else + cv::fillPoly(frame, std::vector>{poly}, cv::Scalar(0, 0, 255)); + } + } + + // Creates a separate mask only for the danger zone + void createDangerZoneMask(const cv::Mat &original_mask, + const std::vector &left_curve, + const std::vector &right_curve, + float displacement_cm, float scale, + cv::Mat &danger_zone_mask) + { + // Initialize danger zone mask as zeros + danger_zone_mask = cv::Mat::zeros(original_mask.size(), CV_8UC1); + + if (original_mask.empty()) + return; + + const int start_y = original_mask.rows * 2 / 5; // Lower 3/5 of frame + const int width = original_mask.cols; + const int end_y = original_mask.rows - 1; + const int min_pts = 8; // avoids degenerate polygons + const int lane_width_pixels = std::max(1, int(displacement_cm / scale)); + + std::vector poly; + + if (!left_curve.empty() && !right_curve.empty()) + { + // left: from bottom to top (large y -> small) + for (auto it = left_curve.rbegin(); it != left_curve.rend(); ++it) + if (it->y >= start_y && it->y <= end_y) + poly.push_back(*it); + + // right: from top to bottom (small y -> large) + for (const auto &p : right_curve) + if (p.y >= start_y && p.y <= end_y) + poly.push_back(p); + } + else if (!right_curve.empty()) + { + // only right → estimate left by offset + for (auto it = right_curve.rbegin(); it != right_curve.rend(); ++it) + { + if (it->y >= start_y && it->y <= end_y) + { + int xl = std::max(0, it->x - lane_width_pixels); + poly.emplace_back(xl, it->y); + } + } + for (const auto &p : right_curve) + if (p.y >= start_y && p.y <= end_y) + poly.push_back(p); + } + else if (!left_curve.empty()) + { + // only left → estimate right by offset + for (const auto &p : left_curve) + if (p.y >= start_y && p.y <= end_y) + poly.push_back(p); + + for (auto it = left_curve.rbegin(); it != left_curve.rend(); ++it) + { + if (it->y >= start_y && it->y <= end_y) + { + int xr = std::min(width - 1, it->x + lane_width_pixels); + poly.emplace_back(xr, it->y); + } + } + } + + // Create filled danger zone polygon + if ((int)poly.size() >= min_pts) + { + cv::fillPoly(danger_zone_mask, std::vector>{poly}, cv::Scalar(255)); + } + + // DO NOT apply bitwise_and - we want to show the filled danger zone, not just the obstacles + // The mask now shows the complete danger zone area in white + } + + // Calculates danger zone occupancy based on the danger zone mask + float calculateDangerZoneOccupancyFromMask(const cv::Mat &original_mask, + const cv::Mat &danger_zone_mask) + { + if (original_mask.empty() || danger_zone_mask.empty()) + { + return 0.0f; + } + + // Calculate total danger zone area (white pixels in danger zone mask) + int total_danger_area = cv::countNonZero(danger_zone_mask); + if (total_danger_area == 0) + { + return 0.0f; + } + + // Apply danger zone mask to original mask to get only obstacles in the zone + cv::Mat obstacles_in_danger_zone; + cv::bitwise_and(original_mask, danger_zone_mask, obstacles_in_danger_zone); + + // Count white pixels (obstacles) in danger zone + int obstacle_pixels = cv::countNonZero(obstacles_in_danger_zone); + + // Calculate occupancy percentage + float occupancy = static_cast(obstacle_pixels) / static_cast(total_danger_area); + + return occupancy; + } + + // ====== FUNCTIONS FOR DRIVABLE AREA DANGER ZONE ====== + + // Creates a danger zone mask based on the drivable area + void createDrivableDangerZoneMask(const cv::Mat &drivable_mask, + float displacement_cm, float scale, + cv::Mat &drivable_danger_zone_mask) + { + // Initialize danger zone mask as zeros + drivable_danger_zone_mask = cv::Mat::zeros(drivable_mask.size(), CV_8UC1); + + // Always create danger zone, even if drivable mask is empty + // This allows detecting obstacles even when no drivable area is detected + + const int start_y = drivable_mask.rows * 2 / 5; // Lower 3/5 of frame + const int end_y = drivable_mask.rows - 1; + const int width = drivable_mask.cols; + const int center_x = width / 2; + const int danger_width_pixels = std::max(10, int(displacement_cm / scale)); + + // Create rectangular danger zone in the lower central part + cv::Rect danger_zone_rect( + center_x - danger_width_pixels / 2, // x + start_y, // y + danger_width_pixels, // width + end_y - start_y // height + ); + + // Ensure rectangle is within image bounds + danger_zone_rect &= cv::Rect(0, 0, width, drivable_mask.rows); + + // Fill danger zone in mask + if (danger_zone_rect.area() > 0) + { + cv::rectangle(drivable_danger_zone_mask, danger_zone_rect, cv::Scalar(255), -1); + } + } + + // Calculates drivable danger zone occupancy (detected obstacles) + float calculateDrivableDangerZoneOccupancy(const cv::Mat &drivable_mask, + const cv::Mat &drivable_danger_zone_mask) + { + if (drivable_danger_zone_mask.empty()) + { + return 0.0f; + } + + // Calculate total danger zone area + int total_danger_area = cv::countNonZero(drivable_danger_zone_mask); + if (total_danger_area == 0) + { + return 0.0f; + } + + // If drivable mask is empty, consider entire danger zone as obstacle + if (drivable_mask.empty() || cv::countNonZero(drivable_mask) == 0) + { + // Entire danger zone is considered obstacle (100% occupancy) + return 1.0f; + } + + // Detect obstacles: areas where drivable mask is 0 (not drivable) within danger zone + cv::Mat obstacles_in_danger_zone; + cv::bitwise_and(~drivable_mask, drivable_danger_zone_mask, obstacles_in_danger_zone); + + // Count obstacle pixels in danger zone + int obstacle_pixels = cv::countNonZero(obstacles_in_danger_zone); + + // Calculate occupancy percentage + float occupancy = static_cast(obstacle_pixels) / static_cast(total_danger_area); + + return occupancy; + } + + // Draws drivable danger zone on frame + void drawDrivableDangerZone(cv::Mat &frame, + const cv::Mat &drivable_danger_zone_mask) + { + if (frame.empty() || drivable_danger_zone_mask.empty()) + return; + + // Convert to 3 channels if necessary + cv::Mat frame_3ch; + if (frame.channels() == 1) + { + cv::cvtColor(frame, frame_3ch, cv::COLOR_GRAY2BGR); + } + else + { + frame_3ch = frame; + } + + // Create colored mask for drivable danger zone (blue) + cv::Mat danger_overlay = cv::Mat::zeros(frame_3ch.size(), CV_8UC3); + danger_overlay.setTo(cv::Scalar(255, 0, 0), drivable_danger_zone_mask); // BGR: blue + + // Apply transparency and overlay + cv::addWeighted(frame_3ch, 0.7, danger_overlay, 0.3, 0, frame_3ch); + + // Copy back to original frame + if (frame.channels() == 1) + { + cv::cvtColor(frame_3ch, frame, cv::COLOR_BGR2GRAY); + } + else + { + frame = frame_3ch; + } + } +} // namespace jetracer::vision diff --git a/emergency_stop/sources/i2c_device.cpp b/emergency_stop/sources/i2c_device.cpp new file mode 100644 index 000000000..a338d924e --- /dev/null +++ b/emergency_stop/sources/i2c_device.cpp @@ -0,0 +1,59 @@ +#include "jetracer/i2c_device.hpp" +#include +#include +#include +#include +#include + +namespace jetracer::hardware +{ + + I2CDevice::I2CDevice(const std::string &device, int address) + { + fd_ = open(device.c_str(), O_RDWR); + if (fd_ < 0) + { + throw std::runtime_error("Failed to open I2C device: " + device); + } + if (ioctl(fd_, I2C_SLAVE, address) < 0) + { + close(fd_); + throw std::runtime_error("Failed to set I2C address"); + } + } + + I2CDevice::~I2CDevice() + { + if (fd_ >= 0) + { + close(fd_); + } + } + + void I2CDevice::write_byte(uint8_t reg, uint8_t value) + { + uint8_t buffer[2] = {reg, value}; + if (write(fd_, buffer, 2) != 2) + { + close(fd_); + throw std::runtime_error("Failed to write to I2C device"); + } + } + + uint8_t I2CDevice::read_byte(uint8_t reg) + { + if (write(fd_, ®, 1) != 1) + { + close(fd_); + throw std::runtime_error("Failed to write register to I2C device"); + } + uint8_t value; + if (read(fd_, &value, 1) != 1) + { + close(fd_); + throw std::runtime_error("Failed to read from I2C device"); + } + return value; + } + +} // namespace jetracer::hardware diff --git a/emergency_stop/sources/jetracer.cpp b/emergency_stop/sources/jetracer.cpp new file mode 100644 index 000000000..e85c10059 --- /dev/null +++ b/emergency_stop/sources/jetracer.cpp @@ -0,0 +1,939 @@ +#include "jetracer/jetracer.hpp" +#include +#include +#include +#include + +namespace jetracer::control +{ + JetRacer::JetRacer(int servo_addr, int motor_addr) + : servo_addr_(servo_addr), + motor_addr_(motor_addr), + running_(false), + servo_device_("/dev/i2c-1", servo_addr), + motor_device_("/dev/i2c-1", motor_addr) + { + init_servo(); + init_motors(); + } + + JetRacer::~JetRacer() + { + stop(); + } + + void JetRacer::init_servo() + { + try + { + servo_device_.write_byte(0x00, 0x06); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0x00, 0x10); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0xFE, 0x79); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0x01, 0x04); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + servo_device_.write_byte(0x00, 0x20); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + catch (const std::exception &e) + { + std::cerr << "Servo initialization failed: " << e.what() << std::endl; + stop(); + } + } + + void JetRacer::init_motors() + { + try + { + motor_device_.write_byte(0x00, 0x20); + + int prescale = static_cast(std::floor(25000000.0 / 4096.0 / PWM_FREQUENCY_HZ - 1)); + int oldmode = motor_device_.read_byte(0x00); + int newmode = (oldmode & 0x7F) | 0x10; + + motor_device_.write_byte(0x00, newmode); + motor_device_.write_byte(0xFE, prescale); + motor_device_.write_byte(0x00, oldmode); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + motor_device_.write_byte(0x00, oldmode | 0xA1); + + std::cout << "[INFO] Motors initialized with PWM frequency: " << PWM_FREQUENCY_HZ << " Hz" << std::endl; + std::cout << "[INFO] Calculated prescale: " << prescale << std::endl; + } + catch (const std::exception &e) + { + std::cerr << "Motor initialization failed: " << e.what() << std::endl; + stop(); + } + } + + void JetRacer::set_steering(int angle) + { + angle = std::clamp(angle, -MAX_ANGLE_, MAX_ANGLE_); + + int pwm = 0; + if (angle < 0) + { + std::cout << "Setting steering to left: " << angle << std::endl; + pwm = SERVO_CENTER_PWM_ + (angle / static_cast(MAX_ANGLE_)) * (SERVO_CENTER_PWM_ - SERVO_LEFT_PWM_); + } + else if (angle > 0) + { + pwm = SERVO_CENTER_PWM_ + (angle / static_cast(MAX_ANGLE_)) * (SERVO_RIGHT_PWM_ - SERVO_CENTER_PWM_); + std::cout << "Setting steering to right: " << angle << std::endl; + } + else + { + pwm = SERVO_CENTER_PWM_; + std::cout << "Setting steering to center: " << angle << std::endl; + } + + set_servo_pwm(0, 0, pwm); + current_angle_ = angle; + + std::this_thread::sleep_for(std::chrono::milliseconds(servo_delay_ms_)); + } + + void JetRacer::smooth_steering(int target_angle, int increment) + { + target_angle = std::clamp(target_angle, -MAX_ANGLE_, MAX_ANGLE_); + int step = (target_angle > current_angle_) ? increment : -increment; + + while ((step > 0 && current_angle_ < target_angle) || (step < 0 && current_angle_ > target_angle)) + { + current_angle_ += step; + if ((step > 0 && current_angle_ > target_angle) || (step < 0 && current_angle_ < target_angle)) + { + current_angle_ = target_angle; + } + set_steering(current_angle_); + } + } + + void JetRacer::set_servo_pwm(int channel, int on_value, int off_value) + { + int base_reg = 0x06 + (channel * 4); + servo_device_.write_byte(base_reg, on_value & 0xFF); + servo_device_.write_byte(base_reg + 1, on_value >> 8); + servo_device_.write_byte(base_reg + 2, off_value & 0xFF); + servo_device_.write_byte(base_reg + 3, off_value >> 8); + } + + void JetRacer::set_motor_pwm(int channel, int value) + { + value = std::clamp(value, 0, 4095); + int base_reg = 0x06 + (channel * 4); + motor_device_.write_byte(base_reg, 0); + motor_device_.write_byte(base_reg + 1, 0); + motor_device_.write_byte(base_reg + 2, value & 0xFF); + motor_device_.write_byte(base_reg + 3, value >> 8); + } + + float JetRacer::smooth_speed(float target_speed) + { + // Add new target speed to history + speed_history_.push_back(target_speed); + + // Keep only the last SPEED_SMOOTHING_WINDOW speeds + if (speed_history_.size() > SPEED_SMOOTHING_WINDOW) + { + speed_history_.pop_front(); + } + + // Calculate average of speeds in history + float sum = 0.0f; + for (float speed : speed_history_) + { + sum += speed; + } + float average_speed = sum / speed_history_.size(); + + // Apply maximum change limitation per update + float max_change = MAX_SPEED_CHANGE_PER_UPDATE; + float speed_diff = average_speed - smoothed_speed_; + + if (std::abs(speed_diff) > max_change) + { + if (speed_diff > 0) + { + smoothed_speed_ += max_change; + } + else + { + smoothed_speed_ -= max_change; + } + } + else + { + smoothed_speed_ = average_speed; + } + + return smoothed_speed_; + } + + float JetRacer::calculate_safe_speed(float target_speed) + { + // Apply base speed limitation (as before) + float max_safe_speed = motor_control::safety::BASE_MAX_SPEED; + + // Check if turning based on current steering angle + bool is_turning = std::abs(current_angle_) > motor_control::safety::STEERING_ANGLE_THRESHOLD; + + // CHECK IF CAR IS STOPPED - if so, allows maximum speed to overcome inertia + bool is_car_stopped = (std::abs(current_speed_) < 1.0f); + + if (is_turning && !is_car_stopped) + { + // In curve AND car moving: applies boost to overcome resistance of turned wheels + max_safe_speed *= motor_control::safety::CURVE_SPEED_REDUCTION; + + // Debug log for curves + static int curve_debug_counter = 0; + if ((++curve_debug_counter % 50) == 0) + { // Log every ~2.5 seconds + std::cout << "[CURVE] Angle: " << current_angle_ + << "°, Max speed: " << max_safe_speed << "% (boost applied to overcome resistance)" << std::endl; + } + } + else if (!is_turning) + { + // Straight: normal speed without boost + max_safe_speed *= motor_control::safety::STRAIGHT_SPEED_BOOST; + + // Debug log for straight lines + static int straight_debug_counter = 0; + if ((++straight_debug_counter % 100) == 0) + { // Log every ~5 seconds + std::cout << "[STRAIGHT] Angle: " << current_angle_ + << "°, Max speed: " << max_safe_speed << "% (normal speed)" << std::endl; + } + } + else + { + // Car stopped in curve: allows maximum speed to overcome inertia + if (is_car_stopped && is_turning) + { + static int startup_curve_debug_counter = 0; + if ((++startup_curve_debug_counter % 30) == 0) + { // Log a cada ~1.5 segundos + std::cout << "[STARTUP_CURVE] Carro parado em curva - permitindo velocidade máxima para vencer inércia das rodas viradas" << std::endl; + } + } + } + + // Apply safe speed limitation + if (std::abs(target_speed) > max_safe_speed) + { + // Keep the sign (forward/backward) but limit the magnitude + float sign = (target_speed > 0) ? 1.0f : -1.0f; + target_speed = sign * max_safe_speed; + + // Log applied limitation + static int limit_debug_counter = 0; + if ((++limit_debug_counter % 30) == 0) + { // Log a cada ~1.5 segundos + std::cout << "[SAFETY] Velocidade limitada para " << target_speed + << "% (máximo seguro: " << max_safe_speed << "%)" << std::endl; + } + } + + return target_speed; + } + + void JetRacer::set_motor_pwm_smooth(int channel, int value) + { + // PWM implementation with smoothing for smoother transitions + value = std::clamp(value, 0, 4095); + int base_reg = 0x06 + (channel * 4); + + // CHECK IF CAR IS STOPPED - if so, apply PWM directly without smoothing + bool is_car_stopped = (std::abs(current_speed_) < 1.0f); + + if (is_car_stopped) + { + // CAR REALLY STOPPED: Apply PWM directly for maximum responsiveness + motor_device_.write_byte(base_reg, 0); + motor_device_.write_byte(base_reg + 1, 0); + motor_device_.write_byte(base_reg + 2, value & 0xFF); + motor_device_.write_byte(base_reg + 3, value >> 8); + return; + } + + // CAR IN MOTION: Apply normal smoothing + static std::array last_pwm_values = {0}; + static std::array target_pwm_values = {0}; + + // Define o valor alvo + target_pwm_values[channel] = value; + + // Calculate difference for smoothing + int current_pwm = last_pwm_values[channel]; + int pwm_diff = target_pwm_values[channel] - current_pwm; + + // Apply smoothing with maximum change rate + int max_pwm_change = 200; // Maximum PWM change per update + if (std::abs(pwm_diff) > max_pwm_change) + { + if (pwm_diff > 0) + { + current_pwm += max_pwm_change; + } + else + { + current_pwm -= max_pwm_change; + } + } + else + { + current_pwm = target_pwm_values[channel]; + } + + // Atualiza o valor atual + last_pwm_values[channel] = current_pwm; + + // Aplica o PWM suavizado + motor_device_.write_byte(base_reg, 0); + motor_device_.write_byte(base_reg + 1, 0); + motor_device_.write_byte(base_reg + 2, current_pwm & 0xFF); + motor_device_.write_byte(base_reg + 3, current_pwm >> 8); + } + + void JetRacer::set_speed(float speed) + { + // CHECK IF MOTOR IS LOCKED - if so, ignore speed commands + if (motor_locked_) + { + // Log occasionally to avoid spam + static int motor_lock_debug_counter = 0; + if ((++motor_lock_debug_counter % 100) == 0) + { // Log a cada ~5 segundos + std::cout << "[MOTOR LOCK] Motor travado - ignorando comando de velocidade: " << speed << "%" << std::endl; + } + return; // Exit function without processing command + } + + // CHECK IF STILL IN EMERGENCY BRAKING PERIOD + if (is_emergency_braking_active()) + { + // Apply continuous braking for 2 seconds + for (int channel = 0; channel < 9; ++channel) + { + set_motor_pwm(channel, 4095); // Maximum PWM to maintain braking + } + + // Log occasionally to avoid spam + static int emergency_brake_debug_counter = 0; + if ((++emergency_brake_debug_counter % 50) == 0) + { // Log every ~2.5 seconds + std::cout << "[EMERGENCY BRAKE] Frenagem ativa - aplicando PWM máximo para travar motor" << std::endl; + } + return; // Exit function without processing speed command + } + + // DEBUG: Function entry log + static int set_speed_debug_counter = 0; + + // Speed log + if ((++set_speed_debug_counter % 20) == 0) + { // Log every ~1 second + std::cout << "[DEBUG] set_speed() - Velocidade solicitada: " << speed << "%" << std::endl; + } + + // SAFETY LIMITATION APPLICATION (as before, but more intelligent) + float original_speed = speed; + speed = calculate_safe_speed(speed); + + // Timestamp for stopped state detection + unsigned long current_time = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + + // Log applied limitation (if there was change) + if (std::abs(original_speed) != std::abs(speed) && set_speed_debug_counter % 20 == 0) + { + std::cout << "[SAFETY] Velocidade ajustada de " << original_speed + << "% para " << speed << "% por segurança" << std::endl; + } + + // EMERGENCY BRAKING DETECTION + bool emergency_brake = false; + float speed_reduction = std::abs(speed) - std::abs(last_speed_command_); + + // Detect if there is a sharp speed reduction (>50% reduction) + if (last_speed_command_ != 0.0f && speed_reduction < -motor_control::ramps::EMERGENCY_BRAKE_THRESHOLD) + { + emergency_brake = true; + // std::cout << "[EMERGENCY BRAKE] Frenagem de emergência detectada! Redução: " << speed_reduction << "%" << std::endl; + } + + // Detect rapid direction change (forward to backward or vice-versa) + if ((last_speed_command_ > motor_control::safety::DIRECTION_CHANGE_THRESHOLD && speed < -motor_control::safety::DIRECTION_CHANGE_THRESHOLD) || + (last_speed_command_ < -motor_control::safety::DIRECTION_CHANGE_THRESHOLD && speed > motor_control::safety::DIRECTION_CHANGE_THRESHOLD)) + { + emergency_brake = true; + // std::cout << "[EMERGENCY BRAKE] Mudança de direção detectada!" << std::endl; + } + + // Apply deadzone to avoid oscillations at low speed + if (std::abs(speed) < motor_control::thresholds::SPEED_DEADZONE * 100.0f) + { + speed = 0.0f; + } + + // Simple logic to detect stopped state + bool is_car_stopped = (std::abs(current_speed_) < 1.0f); + + // SPECIAL LOG FOR STARTUP - show when car is really stopped + if (is_car_stopped && std::abs(speed) > 5.0f && set_speed_debug_counter % 10 == 0) + { + std::cout << "[STARTUP] Carro parado - aplicando aceleração direta: " << speed << "%" << std::endl; + } + + // Amplify motor force - intelligent limitation already applied above + // Speed has already been limited by the calculate_safe_speed() function + + // Intelligent power curve reactivated + float power_curve = 1.0f; + if (std::abs(speed) > 0.0f) + { + // Exponential curve for better response at low speeds + power_curve = 1.0f + (std::abs(speed) / 100.0f) * motor_control::power::POWER_CURVE_FACTOR; + speed *= power_curve; + } + + speed = std::max(-100.0f, std::min(speed, 100.0f)); + + // SPECIAL PROCESSING FOR EMERGENCY BRAKING AND STARTUP + if (emergency_brake) + { + // Skip smoothing and apply immediate braking + filtered_speed_ = speed; + target_speed_ = speed; + // std::cout << "[EMERGENCY BRAKE] Aplicando frenagem imediata!" << std::endl; + } + else if (is_car_stopped) + { + // CAR REALLY STOPPED: APPLY COMMANDS DIRECTLY WITHOUT SMOOTHING + // Check if last command was also close to zero (car really stopped) + filtered_speed_ = speed; + target_speed_ = speed; + if (set_speed_debug_counter % 10 == 0) + { + std::cout << "[STARTUP] Carro realmente parado - aplicando aceleração direta: " << speed << "%" << std::endl; + } + } + else if (std::abs(speed - current_speed_) > 5.0f) + { + // SHARP SPEED CHANGE: apply command directly for maximum responsiveness + filtered_speed_ = speed; + target_speed_ = speed; + if (set_speed_debug_counter % 10 == 0) + { + std::cout << "[RESPONSIVE] Mudança brusca detectada - aplicando comando direto: " << speed << "%" << std::endl; + } + } + else + { + // NORMAL INTELLIGENT SMOOTHING (only for small changes) + filtered_speed_ = motor_control::power::SPEED_SMOOTHING_FACTOR * speed + + (1.f - motor_control::power::SPEED_SMOOTHING_FACTOR) * filtered_speed_; + } + + // INTELLIGENT speed ramp with aggressive braking and responsive startup + if (!emergency_brake && !is_car_stopped && std::abs(speed - current_speed_) <= 5.0f) + { + // ONLY apply ramp for small and gradual changes + float speed_diff = filtered_speed_ - target_speed_; + float ramp_rate; + + // Determine type of change: acceleration or deceleration + bool is_braking = (filtered_speed_ < target_speed_ && target_speed_ > 0) || + (filtered_speed_ > target_speed_ && target_speed_ < 0) || + (std::abs(filtered_speed_) < std::abs(target_speed_)); + + if (is_braking) + { + // BRAKING: use very fast deceleration rate + ramp_rate = motor_control::ramps::DECELERATION_RAMP; + } + else + { + // ACCELERATION: use normal rate + ramp_rate = motor_control::ramps::ACCELERATION_RAMP; + } + + if (std::abs(speed_diff) > ramp_rate) + { + target_speed_ += (speed_diff > 0 ? ramp_rate : -ramp_rate); + } + else + { + target_speed_ = filtered_speed_; + } + } + else + { + // For sharp changes or startup: apply directly + target_speed_ = filtered_speed_; + if (set_speed_debug_counter % 10 == 0) + { + std::cout << "[DIRECT] Aplicando comando direto (sem rampa): " << filtered_speed_ << "%" << std::endl; + } + } + + // Converter para PWM com melhor aproveitamento da faixa e threshold otimizado + int pwm_value = static_cast(std::abs(target_speed_) / 100.0f * 4095); + + // DEBUG: Log do PWM calculado + if (set_speed_debug_counter % 20 == 0) + { + std::cout << "[DEBUG] set_speed() - PWM calculado: " << pwm_value + << " para velocidade: " << target_speed_ << "%" << std::endl; + } + + // Apply higher minimum threshold to ensure initial force + if (pwm_value > 0 && pwm_value < motor_control::thresholds::MIN_PWM_THRESHOLD) + { + pwm_value = motor_control::thresholds::MIN_PWM_THRESHOLD; + } + + // FORCED AMPLIFICATION for very small commands (ensure movement) + if (pwm_value > 0 && pwm_value < 1000) + { + pwm_value = static_cast(pwm_value * 2.0f); // 100% additional amplification + } + + // Intelligent torque amplification reactivated + if (pwm_value > 0 && pwm_value < motor_control::thresholds::TORQUE_BOOST_THRESHOLD) + { + pwm_value = static_cast(pwm_value * motor_control::thresholds::LOW_SPEED_AMPLIFICATION); + } + + // SPECIAL BOOST FOR STARTUP (when car is really stopped) + if (is_car_stopped && pwm_value > 0) + { + // Apply additional boost to overcome initial inertia (reduced for slower car) + float boost_multiplier = 2.0f; // Base boost reduced to 100% + + // EXTRA BOOST for cars stopped in curves (harder to get out of place) + if (std::abs(current_angle_) > motor_control::safety::STEERING_ANGLE_THRESHOLD) + { + boost_multiplier = 3.5f; // 250% boost for curves (increased to overcome resistance of turned wheels) + static int curve_startup_debug_counter = 0; + if ((++curve_startup_debug_counter % 10) == 0) + { // Log every ~0.5 seconds + std::cout << "[STARTUP_CURVE] Aplicando boost extra para carro parado em curva: PWM " << pwm_value << std::endl; + } + } + + pwm_value = static_cast(pwm_value * boost_multiplier); + + // Log de debug para partida + static int startup_debug_counter = 0; + if ((++startup_debug_counter % 10) == 0) + { // Log a cada ~0.5 segundos + std::cout << "[STARTUP] Aplicando boost de partida para carro realmente parado: PWM " << pwm_value << std::endl; + } + } + + // Ensure it doesn't exceed maximum + pwm_value = std::min(pwm_value, 4095); + + // DEBUG: Log of calculated PWM values + if (set_speed_debug_counter % 20 == 0) + { + std::cout << "[DEBUG] set_speed() - Target: " << target_speed_ + << "%, PWM: " << pwm_value + << ", Filtered: " << filtered_speed_ << "%" << std::endl; + } + + // Controle suave mas responsivo reativado + if (target_speed_ > 0) + { + set_motor_pwm_smooth(0, pwm_value); + set_motor_pwm_smooth(1, 0); + set_motor_pwm_smooth(2, pwm_value); + set_motor_pwm_smooth(5, pwm_value); + set_motor_pwm_smooth(6, 0); + set_motor_pwm_smooth(7, pwm_value); + } + else if (target_speed_ < 0) + { + set_motor_pwm_smooth(0, pwm_value); + set_motor_pwm_smooth(1, pwm_value); + set_motor_pwm_smooth(2, 0); + set_motor_pwm_smooth(6, pwm_value); + set_motor_pwm_smooth(7, pwm_value); + set_motor_pwm_smooth(8, 0); + } + else + { + // Parada suave com rampa + for (int channel = 0; channel < 9; ++channel) + { + set_motor_pwm_smooth(channel, 0); + } + } + + current_speed_ = target_speed_; + last_speed_command_ = speed; + + // Atualizar timestamp de movimento (simplificado) + if (std::abs(speed) > 1.0f) + { + last_movement_time_ = current_time; + } + } + + // ===== METHODS FOR CONSTANT SPEED TESTING ===== + + void JetRacer::set_constant_speed_mode(bool enabled) + { + test_mode_ = enabled; + if (enabled) + { + std::cout << "[TEST MODE] Modo de teste de velocidade constante ATIVADO" << std::endl; + std::cout << "[TEST MODE] Use set_test_speed() e start_speed_test() para testar" << std::endl; + } + else + { + std::cout << "[TEST MODE] Modo de teste DESATIVADO - voltando ao controle por joystick" << std::endl; + stop_speed_test(); + } + } + + void JetRacer::set_test_speed(float speed_percent) + { + // Limit test speed for safety + speed_percent = std::max(-50.0f, std::min(50.0f, speed_percent)); + test_speed_ = speed_percent; + std::cout << "[TEST MODE] Velocidade de teste definida para: " << speed_percent << "%" << std::endl; + } + + void JetRacer::set_test_duration(int seconds) + { + test_duration_ = std::max(1, std::min(300, seconds)); // 1 segundo a 5 minutos + std::cout << "[TEST MODE] Duração do teste definida para: " << test_duration_ << " segundos" << std::endl; + } + + void JetRacer::start_speed_test() + { + if (!test_mode_) + { + std::cout << "[ERROR] Modo de teste não está ativado. Use set_constant_speed_mode(true) primeiro." << std::endl; + return; + } + + if (test_running_) + { + std::cout << "[WARNING] Teste já está em execução. Parando teste anterior..." << std::endl; + stop_speed_test(); + } + + test_running_ = true; + test_start_time_ = std::chrono::steady_clock::now(); + + // Iniciar thread de teste + test_thread_ = std::thread(&JetRacer::process_test_mode, this); + test_thread_.detach(); + + std::cout << "[TEST MODE] Teste iniciado com velocidade: " << test_speed_ << "% por " << test_duration_ << " segundos" << std::endl; + std::cout << "[TEST MODE] Use stop_speed_test() para parar o teste" << std::endl; + } + + void JetRacer::stop_speed_test() + { + if (test_running_) + { + test_running_ = false; + set_speed(0); // Parar o carro + std::cout << "[TEST MODE] Teste parado. Carro parado." << std::endl; + } + } + + void JetRacer::process_test_mode() + { + std::cout << "[TEST MODE] Aplicando velocidade constante: " << test_speed_ << "%" << std::endl; + + // Aplicar velocidade de teste + set_speed(test_speed_); + + // Wait for test duration + auto start_time = std::chrono::steady_clock::now(); + while (test_running_) + { + auto current_time = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(current_time - start_time).count(); + + if (elapsed >= test_duration_) + { + std::cout << "[TEST MODE] Duração do teste atingida (" << test_duration_ << "s). Parando..." << std::endl; + break; + } + + // Manter velocidade constante + set_speed(test_speed_); + + // Wait before next update + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Parar o carro ao final do teste + if (test_running_) + { + set_speed(0); + test_running_ = false; + std::cout << "[TEST MODE] Teste finalizado. Carro parado." << std::endl; + } + } + + void JetRacer::process_joystick() + { + if (SDL_Init(SDL_INIT_JOYSTICK) < 0) + { + std::cerr << "Failed to initialize SDL: " << SDL_GetError() << std::endl; + return; + } + + SDL_Joystick *joystick = SDL_JoystickOpen(0); + if (!joystick) + { + std::cerr << "Failed to open joystick: " << SDL_GetError() << std::endl; + SDL_Quit(); + return; + } + + while (running_) + { + // Check if in test mode - if so, don't process joystick + if (test_mode_ && test_running_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + + // DEBUG: Log to verify if joystick is being processed + static int debug_counter = 0; + if ((++debug_counter % 100) == 0) + { // Log every ~2.5 seconds + std::cout << "[JOYSTICK] Processando entrada do joystick..." << std::endl; + } + + SDL_JoystickUpdate(); + + int left_joystick_y = SDL_JoystickGetAxis(joystick, 1); // Speed control + // int right_joystick_x = SDL_JoystickGetAxis(joystick, 2); // Directional control + + // Detect R2 button (button 7 on standard controller) + bool r2_current = SDL_JoystickGetButton(joystick, 7); + + // Detect R2 click (transition from not pressed to pressed) + if (r2_current && !r2_button_was_pressed_) + { + // Toggle do cruise control + if (cruise_control_active_) + { + // Desativar cruise control + cruise_control_active_ = false; + std::cout << "[CRUISE CONTROL] Desativado - Retornando ao controle manual" << std::endl; + } + else + { + // Ativar cruise control com a velocidade atual + float current_speed = -left_joystick_y / 32767.0f * 100; + if (std::abs(current_speed) > 5.0f) + { // Only activate if there is significant speed + cruise_control_active_ = true; + cruise_control_speed_ = current_speed; + std::cout << "[CRUISE CONTROL] Ativado - Velocidade: " << current_speed << "%" << std::endl; + } + else + { + std::cout << "[CRUISE CONTROL] Não ativado - Velocidade muito baixa: " << current_speed << "%" << std::endl; + } + } + } + r2_button_was_pressed_ = r2_current; + + // DEBUG: Log dos valores do joystick + if (debug_counter % 100 == 0) + { + float speed_percent = -left_joystick_y / 32767.0f * 100; + std::cout << "[JOYSTICK] Y: " << left_joystick_y << " -> Velocidade: " << speed_percent << "%"; + if (cruise_control_active_) + { + std::cout << " [CRUISE: " << cruise_control_speed_ << "%]"; + } + std::cout << std::endl; + } + + // Aplicar velocidade baseada no modo atual + if (cruise_control_active_) + { + // Modo cruise control - usar velocidade salva + set_speed(cruise_control_speed_); + } + else + { + // Modo manual - usar joystick + set_speed(-left_joystick_y / 32767.0f * 100); + } + // smooth_steering(right_joystick_x / 32767.0f * MAX_ANGLE_, 10); + + // Configurable update frequency for smoother response + std::this_thread::sleep_for(std::chrono::milliseconds(pwm::timing::JOYSTICK_UPDATE_MS)); + } + + SDL_JoystickClose(joystick); + SDL_Quit(); + } + + void JetRacer::start() + { + running_ = true; + std::thread joystick_thread(&JetRacer::process_joystick, this); + joystick_thread.detach(); + } + + void JetRacer::stop() + { + running_ = false; + stop_speed_test(); // Parar teste se estiver rodando + set_speed(0); + set_steering(0); + } + + bool JetRacer::is_running() const + { + return running_.load(); + } + + void JetRacer::emergency_stop() + { + std::cout << "[EMERGENCY STOP] Parada de emergência ativada!" << std::endl; + + // TRAVAR O MOTOR para evitar movimento em "ponto morto" + motor_locked_ = true; + std::cout << "[EMERGENCY STOP] Motor travado para evitar movimento em ponto morto" << std::endl; + + // ACTIVATE EMERGENCY BRAKING FOR 3 SECONDS + emergency_braking_active_ = true; + emergency_brake_start_time_ = std::chrono::steady_clock::now(); + std::cout << "[EMERGENCY STOP] Frenagem de emergência ativada por 3 segundos" << std::endl; + + // Apply immediate braking with maximum PWM to lock motor + // Send maximum PWM to both directions to create resistance + for (int channel = 0; channel < 9; ++channel) + { + set_motor_pwm(channel, 4095); // Maximum PWM to lock + set_motor_pwm(channel, 4095); + } + std::cout << "[EMERGENCY STOP] PWM máximo aplicado para travar o motor" << std::endl; + + // Wait a moment to ensure locking is effective + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Now completely stop the motor + for (int channel = 0; channel < 9; ++channel) + { + set_motor_pwm(channel, 0); + } + + // Center the steering + set_steering(0); + + // Stop speed tests if they are active + if (test_mode_) + { + stop_speed_test(); + std::cout << "[EMERGENCY STOP] Teste de velocidade interrompido" << std::endl; + } + + // Desativar cruise control se estiver ativo + if (cruise_control_active_) + { + cruise_control_active_ = false; + std::cout << "[EMERGENCY STOP] Cruise control desativado" << std::endl; + } + + // Reset state variables + current_speed_ = 0.0f; + target_speed_ = 0.0f; + filtered_speed_ = 0.0f; + last_speed_command_ = 0.0f; + + // Log detalhado + std::cout << "[EMERGENCY STOP] Sistema parado - velocidade: 0%, direção: centralizada" << std::endl; + std::cout << "[EMERGENCY STOP] Motor travado - aguardando zona de perigo ficar livre..." << std::endl; + std::cout << "[EMERGENCY STOP] Frenagem ativa por 3 segundos - como se pisasse no freio" << std::endl; + } + + // ====== FUNCTION TO RELEASE MOTOR LOCK ====== + + void JetRacer::release_motor_lock() + { + if (motor_locked_) + { + motor_locked_ = false; + std::cout << "[MOTOR LOCK] Travamento do motor liberado - sistema pronto para movimento" << std::endl; + } + else + { + std::cout << "[MOTOR LOCK] Motor já estava liberado" << std::endl; + } + } + + // ====== FUNCTION TO CHECK IF STILL BRAKING ====== + + bool JetRacer::is_emergency_braking_active() + { + if (!emergency_braking_active_) + { + return false; + } + + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - emergency_brake_start_time_).count(); + + // Se passou dos 3 segundos, desativar a frenagem + if (elapsed >= EMERGENCY_BRAKE_DURATION_MS) + { + emergency_braking_active_ = false; + return false; + } + + return true; + } + + // ====== CRUISE CONTROL FUNCTIONS ====== + + void JetRacer::set_cruise_control_mode(bool enabled) + { + if (enabled && !cruise_control_active_) + { + // Ativar cruise control + cruise_control_active_ = true; + std::cout << "[CRUISE CONTROL] Modo ativado programaticamente" << std::endl; + } + else if (!enabled && cruise_control_active_) + { + // Desativar cruise control + cruise_control_active_ = false; + std::cout << "[CRUISE CONTROL] Modo desativado programaticamente" << std::endl; + } + } + + void JetRacer::set_cruise_control_speed(float speed) + { + cruise_control_speed_ = speed; + std::cout << "[CRUISE CONTROL] Velocidade definida programaticamente: " << speed << "%" << std::endl; + } + +} // namespace jetracer::control diff --git a/emergency_stop/sources/pid_controller.cpp b/emergency_stop/sources/pid_controller.cpp new file mode 100644 index 000000000..dfa5a7299 --- /dev/null +++ b/emergency_stop/sources/pid_controller.cpp @@ -0,0 +1,68 @@ +#include "jetracer/pid_controller.hpp" +#include "jetracer/computer_vision.hpp" +#include + +namespace jetracer::pid +{ + constexpr float Kp = 1.5f; + constexpr float Ki = 0.1f; + constexpr float Kd = 0.2f; + constexpr float MAX_ANGLE = 140.0f; + constexpr float displacement_cm = 17.0f; + + float PIDapply(float error, float dt, PIDStatus &status) + { + status.integral_error += error * dt; + float derivative = (error - status.previous_error) / dt; + status.previous_error = error; + + float output = Kp * error + Ki * status.integral_error + Kd * derivative; + return std::clamp(output, -MAX_ANGLE, MAX_ANGLE); + } + + float PIDexecute(const cv::Mat &original_frame) + { + if (original_frame.empty() || original_frame.channels() != 1) + { + std::cerr << "Invalid input image." << std::endl; + return 0.0f; + } + + cv::Mat frame = original_frame.clone(); + const float image_center = frame.cols / 2.0f; + const float scale = 40.0f / (frame.cols / 2); + + PIDStatus pid_state; + std::vector left, right; + float y_ref = -1.0f; + + bool found = jetracer::vision::extractLanePoints(frame, image_center, y_ref, left, right); + if (!found) + { + std::cerr << "No lane detected." << std::endl; + return 0.0f; + } + + std::string status; + if (left.empty() && right.empty()) + status = "none"; + else + { + if (!left.empty()) + status += "left"; + if (!right.empty()) + status += "right"; + status = "Lane detected: " + status; + } + + float center_track = jetracer::vision::calculateTrackCenter(left, right, y_ref, displacement_cm, scale, frame); + float lateral_error = image_center - center_track; + float error = lateral_error * scale; + float pid_angle = -PIDapply(error, 0.1f, pid_state); + + std::cout << "Lateral error: " << error << " degrees" << std::endl; + std::cout << "PID correction: " << pid_angle << " degrees" << std::endl; + + return pid_angle; + } +} // namespace jetracer::pid diff --git a/pid_control/.vscode/settings.json b/pid_control/.vscode/settings.json deleted file mode 100644 index 59deaaec7..000000000 --- a/pid_control/.vscode/settings.json +++ /dev/null @@ -1,66 +0,0 @@ -{ - "files.associations": { - "array": "cpp", - "atomic": "cpp", - "bit": "cpp", - "cctype": "cpp", - "chrono": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdint": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "deque": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "random": "cpp", - "ratio": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cinttypes": "cpp", - "typeinfo": "cpp" - } -} diff --git a/pid_control/Notes/buttonsReferences b/pid_control/Notes/buttonsReferences deleted file mode 100644 index 2b80200f7..000000000 --- a/pid_control/Notes/buttonsReferences +++ /dev/null @@ -1,70 +0,0 @@ -Output ao executar o comando "jstest /dev/input/js0" e clicar nos botoes/alavancas do controle: -Link para testar o gamepad: https://hardwaretester.com/gamepad - -SETA PARA CIMA = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7:-32767 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off -SETA PARA BAIXO = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 32767 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off -SETA PARA FRENTE = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6:-32767 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off -SETA PARA TRAS = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 32767 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - -ALAVANCA DIREITA: - Para cima = Axes: 0: 0 1: 0 2: 0 3:-32767 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - Para baixo = Axes: 0: 0 1: 0 2: 0 3: 32767 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - Para tras = Axes: 0: 0 1: 0 2:-32767 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - Para frente = Axes: 0: 0 1: 0 2: 32767 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - -R1 = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:on 8:off 9:off 10:off 11:off 12:off 13:off 14:off -R2 = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:on 10:off 11:off 12:off 13:off 14:off - -ALAVANCA ESQUERDA: - Para cima = Axes: 0: 0 1:-32767 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - Para baixo = Axes: 0: 0 1: 32767 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - Para tras = Axes: 0: -9797 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - Para frente = Axes: 0: 32767 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - -L1 = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:on 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off -L2 = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5: 32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:on 9:off 10:off 11:off 12:off 13:off 14:off - -BUTTON Y = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:on 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - -BUTTON X = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:on 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - -BUTTON B = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:on 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - -BUTTON A = Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:on 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off 13:off 14:off - - - -RESUMO: - - SETAS: - - Para CIMA = 7 (< 0) - Para BAIXO = 7 (> 0) - Para FRENTE = 6 (< 0) - Para TRAS = 6 (> 0) - - ALAVANCA DIREITA: - - Para CIMA = 3 (< 0) - Para BAIXO = 3 (> 0) - Para TRAS = 2 (< 0) - Para FRENTE = 2 (> 0) - - ALAVANCA ESQUERDA: - - Para CIMA = 1 (< 0) - Para BAIXO = 1 (> 0) - Para TRAS = 0 (< 0) - Para FRENTE = 0 (> 0) - - BOTOES: - - R1 = 7 (on) - R2 = 9 (on) - L1 = 6 (on) - L2 = 8 (on) - - Y = 4 (on) - X = 3 (on) - B = 1 (on) - A = 0 (on) diff --git a/pid_control/README.md b/pid_control/README.md deleted file mode 100644 index 419ef67e0..000000000 --- a/pid_control/README.md +++ /dev/null @@ -1,308 +0,0 @@ -# JetCar Project - -## Overview - -The **JetCar** project is a C++ library designed to control a robotic car using servo motors, DC motors, and a joystick. The project integrates various hardware components via the I2C protocol and utilizes SDL2 for joystick support. - -Key features include: -- Servo motor control for steering. -- DC motor control for speed. -- Joystick integration for real-time control. -- Threaded execution for concurrent processing. -- Graceful handling of signals for safe shutdown. - ---- - -## Files in the Project - -### 1. `jetracer.hpp` -This header file declares the `JetCar` class, encapsulating the logic to interface with servos, motors, and joysticks. - -### 2. `jetracer.cpp` -Implements the functionality defined in `jetracer.hpp`. - -### 3. `main.cpp` -Contains the main application to run the JetCar control system. Handles signal interruptions to stop the car gracefully. - ---- - -## Requirements - -### Hardware: -- **Servo Motor** for steering. -- **DC Motors** for driving. -- **Joystick** for user input. -- **I2C-compatible Controller** for motor and servo control (e.g., PCA9685). - -### Software: -- C++17 or later. -- SDL2 library. -- Linux-based system with I2C support. - ---- - -## Installation - -1. Clone the repository: - ```bash - git clone - cd JetCar - ``` -2. Install dependencies: - ```bash - sudo apt-get install libsdl2-dev - ``` -3. Compile the project using `g++`: - ```bash - g++ -std=c++17 -o jetracer main.cpp jetracer.cpp -lSDL2 -pthread - ``` - ---- - -## Usage - -1. Ensure the I2C bus is enabled on your system. -2. Run the compiled program: - ```bash - ./jetracer - ``` -3. Use a joystick to control the car's steering and speed. - ---- - -## Class Details - -### JetCar -#### Constructor: -```cpp -JetCar(int servo_addr_ess = 0x40, int motor_addr_ess = 0x60); -``` -- Initializes the servo and motor controllers via I2C. -- Default I2C addresses are 0x40 (servo) and 0x60 (motor). - -#### Public Methods: -1. **`void init_servo();`** - - Configures the servo controller. - -2. **`void init_motors();`** - - Configures the motor controller. - -3. **`void set_steering(int angle);`** - - Adjusts the steering angle (range: -180 to 180 degrees). - -4. **`void set_speed(float speed);`** - - Sets the motor speed (-100 to 100%). - -5. **`void process_joystick();`** - - Reads joystick input for speed and steering. - -6. **`void start();`** - - Starts the car control system in a separate thread. - -7. **`void stop();`** - - Stops the car safely. - -8. **`bool is_running_() const;`** - - Checks if the system is running. - -#### Private Methods: -- Helper functions for I2C communication: - - `write_byte` / `read_byte` - - `write_byte_data` / `read_byte_data` - ---- - -## Example Code - -```cpp -#include "jetracer.hpp" - -JetCar* car_ptr = nullptr; - -void signal_handler(int) { - if (car_ptr) { - car_ptr->stop(); - } -} - -int main() { - try { - JetCar car; - car_ptr = &car; - signal(SIGINT, signal_handler); - car.start(); - while (car.is_running_()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - } catch (const std::exception& e) { - std::cerr << "Error: " << e.what() << std::endl; - return 1; - } - return 0; -} -``` - ---- - -## Notes - -- Ensure correct wiring of the servo and motor controllers to the I2C bus. -- Modify the I2C addresses in the constructor if using different hardware. -- Use a reliable joystick for smooth control. - - ---- - -# Projeto JetCar - -## Visão Geral - -O projeto **JetCar** é uma biblioteca em C++ concebida para controlar um carro robótico utilizando servomotores, motores DC e um joystick. O projeto integra diversos componentes de hardware via o protocolo I2C e utiliza a biblioteca SDL2 para suporte ao joystick. - -Principais funcionalidades incluem: -- Controlo de servomotor para direcção. -- Controlo de motores DC para velocidade. -- Integração de joystick para controlo em tempo real. -- Execução com threads para processamento concorrente. -- Gestão de sinais para encerramento seguro. - ---- - -## Ficheiros no Projeto - -### 1. `jetracer.hpp` -Este ficheiro de cabeçalho declara a classe `JetCar`, que encapsula a lógica para interagir com os servos, motores e joystick. - -### 2. `jetracer.cpp` -Implementa a funcionalidade definida em `jetracer.hpp`. - -### 3. `main.cpp` -Contém a aplicação principal que executa o sistema de controlo JetCar. Garante o encerramento seguro do carro através do tratamento de sinais. - ---- - -## Requisitos - -### Hardware: -- **Servomotor** para controlo da direcção. -- **Motores DC** para movimentação. -- **Joystick** para entrada de utilizador. -- **Controlador compatível com I2C** para controlo de motores e servos (e.g., PCA9685). - -### Software: -- C++17 ou superior. -- Biblioteca SDL2. -- Sistema operativo baseado em Linux com suporte a I2C. - ---- - -## Instalação - -1. Clone o repositório: - ```bash - git clone - cd JetCar - ``` -2. Instale as dependências: - ```bash - sudo apt-get install libsdl2-dev - ``` -3. Compile o projeto utilizando `g++`: - ```bash - g++ -std=c++17 -o jetracer main.cpp jetracer.cpp -lSDL2 -pthread - ``` - ---- - -## Utilização - -1. Certifique-se de que o barramento I2C está ativado no seu sistema. -2. Execute o programa compilado: - ```bash - ./jetracer - ``` -3. Utilize o joystick para controlar a direcção e a velocidade do carro. - ---- - -## Detalhes da Classe - -### JetCar -#### Construtor: -```cpp -JetCar(int servo_addr_ess = 0x40, int motor_addr_ess = 0x60); -``` -- Inicializa os controladores de servo e motor via I2C. -- Endereços I2C padrão: 0x40 (servo) e 0x60 (motor). - -#### Métodos Públicos: -1. **`void init_servo();`** - - Configura o controlador do servomotor. - -2. **`void init_motors();`** - - Configura o controlador dos motores DC. - -3. **`void set_steering(int angle);`** - - Ajusta o ângulo de direcção (intervalo: -180 a 180 graus). - -4. **`void set_speed(float speed);`** - - Define a velocidade do motor (-100 a 100%). - -5. **`void process_joystick();`** - - Lê a entrada do joystick para controlo de velocidade e direcção. - -6. **`void start();`** - - Inicia o sistema de controlo do carro numa thread separada. - -7. **`void stop();`** - - Pára o carro de forma segura. - -8. **`bool is_running_() const;`** - - Verifica se o sistema está em execução. - -#### Métodos Privados: -- Funções auxiliares para comunicação I2C: - - `write_byte` / `read_byte` - - `write_byte_data` / `read_byte_data` - ---- - -## Exemplo de Código - -```cpp -#include "jetracer.hpp" - -JetCar* car_ptr = nullptr; - -void signal_handler(int) { - if (car_ptr) { - car_ptr->stop(); - } -} - -int main() { - try { - JetCar car; - car_ptr = &car; - signal(SIGINT, signal_handler); - car.start(); - while (car.is_running_()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - } catch (const std::exception& e) { - std::cerr << "Erro: " << e.what() << std::endl; - return 1; - } - return 0; -} -``` - ---- - -## Notas - -- Certifique-se de que a cablagem dos controladores de servo e motor está correta no barramento I2C. -- Modifique os endereços I2C no construtor se estiver a utilizar hardware diferente. -- Utilize um joystick fiável para um controlo suave. - ---- diff --git a/pid_control/includes/jetracer/jetracer.hpp b/pid_control/includes/jetracer/jetracer.hpp deleted file mode 100644 index 261732da7..000000000 --- a/pid_control/includes/jetracer/jetracer.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef JETRACER_HPP -#define JETRACER_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include "jetracer/i2c_device.hpp" - -namespace jetracer::control -{ - class JetRacer - { - public: - JetRacer(int servo_addr, int motor_addr); - ~JetRacer(); - - void start(); - void stop(); - bool is_running() const; - void set_speed(float speed); - void set_steering(int angle); - void smooth_steering(int target_angle, int increment); - int servo_delay_ms_ = 30; - - private: - void init_servo(); - void init_motors(); - void set_servo_pwm(int channel, int on_value, int off_value); - void set_motor_pwm(int channel, int value); - void process_joystick(); - - static constexpr int MAX_ANGLE_ = 140; - static constexpr int SERVO_LEFT_PWM_ = 140; - static constexpr int SERVO_CENTER_PWM_ = 280; - static constexpr int SERVO_RIGHT_PWM_ = 420; - - int servo_addr_; - int motor_addr_; - std::atomic running_; - hardware::I2CDevice servo_device_; - hardware::I2CDevice motor_device_; - int current_angle_ = 0; - float current_speed_ = 0.0f; - }; -} // namespace jetracer::control - -#endif // JETRACER_HPP diff --git a/pid_control/sources/i2c_device.cpp b/pid_control/sources/i2c_device.cpp deleted file mode 100644 index 00d4c7a6c..000000000 --- a/pid_control/sources/i2c_device.cpp +++ /dev/null @@ -1,48 +0,0 @@ -#include "jetracer/i2c_device.hpp" -#include -#include -#include -#include -#include - -namespace jetracer::hardware { - -I2CDevice::I2CDevice(const std::string& device, int address) { - fd_ = open(device.c_str(), O_RDWR); - if (fd_ < 0) { - throw std::runtime_error("Failed to open I2C device: " + device); - } - if (ioctl(fd_, I2C_SLAVE, address) < 0) { - close(fd_); - throw std::runtime_error("Failed to set I2C address"); - } -} - -I2CDevice::~I2CDevice() { - if (fd_ >= 0) { - close(fd_); - } -} - -void I2CDevice::write_byte(uint8_t reg, uint8_t value) { - uint8_t buffer[2] = {reg, value}; - if (write(fd_, buffer, 2) != 2) { - close(fd_); - throw std::runtime_error("Failed to write to I2C device"); - } -} - -uint8_t I2CDevice::read_byte(uint8_t reg) { - if (write(fd_, ®, 1) != 1) { - close(fd_); - throw std::runtime_error("Failed to write register to I2C device"); - } - uint8_t value; - if (read(fd_, &value, 1) != 1) { - close(fd_); - throw std::runtime_error("Failed to read from I2C device"); - } - return value; -} - -} // namespace jetracer::hardware diff --git a/pid_control/sources/jetracer.cpp b/pid_control/sources/jetracer.cpp deleted file mode 100644 index 7f8ac7b13..000000000 --- a/pid_control/sources/jetracer.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include "jetracer/jetracer.hpp" -#include -#include -#include -#include - -namespace jetracer::control -{ - JetRacer::JetRacer(int servo_addr, int motor_addr) - : servo_addr_(servo_addr), - motor_addr_(motor_addr), - running_(false), - servo_device_("/dev/i2c-1", servo_addr), - motor_device_("/dev/i2c-1", motor_addr) - { - init_servo(); - init_motors(); - } - - JetRacer::~JetRacer() - { - stop(); - } - - void JetRacer::init_servo() - { - try - { - servo_device_.write_byte(0x00, 0x06); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - servo_device_.write_byte(0x00, 0x10); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - servo_device_.write_byte(0xFE, 0x79); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - servo_device_.write_byte(0x01, 0x04); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - servo_device_.write_byte(0x00, 0x20); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - catch (const std::exception &e) - { - std::cerr << "Servo initialization failed: " << e.what() << std::endl; - stop(); - } - } - - void JetRacer::init_motors() - { - try - { - motor_device_.write_byte(0x00, 0x20); - - int prescale = static_cast(std::floor(25000000.0 / 4096.0 / 100 - 1)); - int oldmode = motor_device_.read_byte(0x00); - int newmode = (oldmode & 0x7F) | 0x10; - - motor_device_.write_byte(0x00, newmode); - motor_device_.write_byte(0xFE, prescale); - motor_device_.write_byte(0x00, oldmode); - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - motor_device_.write_byte(0x00, oldmode | 0xA1); - } - catch (const std::exception &e) - { - std::cerr << "Motor initialization failed: " << e.what() << std::endl; - stop(); - } - } - - void JetRacer::set_steering(int angle) - { - angle = std::clamp(angle, -MAX_ANGLE_, MAX_ANGLE_); - - int pwm = 0; - if (angle < 0) - { - std::cout << "Setting steering to left: " << angle << std::endl; - pwm = SERVO_CENTER_PWM_ + (angle / static_cast(MAX_ANGLE_)) * (SERVO_CENTER_PWM_ - SERVO_LEFT_PWM_); - } - else if (angle > 0) - { - pwm = SERVO_CENTER_PWM_ + (angle / static_cast(MAX_ANGLE_)) * (SERVO_RIGHT_PWM_ - SERVO_CENTER_PWM_); - std::cout << "Setting steering to right: " << angle << std::endl; - } - else - { - pwm = SERVO_CENTER_PWM_; - std::cout << "Setting steering to center: " << angle << std::endl; - } - - set_servo_pwm(0, 0, pwm); - current_angle_ = angle; - - std::this_thread::sleep_for(std::chrono::milliseconds(servo_delay_ms_)); - } - - void JetRacer::smooth_steering(int target_angle, int increment) - { - target_angle = std::clamp(target_angle, -MAX_ANGLE_, MAX_ANGLE_); - int step = (target_angle > current_angle_) ? increment : -increment; - - while ((step > 0 && current_angle_ < target_angle) || (step < 0 && current_angle_ > target_angle)) - { - current_angle_ += step; - if ((step > 0 && current_angle_ > target_angle) || (step < 0 && current_angle_ < target_angle)) - { - current_angle_ = target_angle; - } - set_steering(current_angle_); - } - } - - void JetRacer::set_servo_pwm(int channel, int on_value, int off_value) - { - int base_reg = 0x06 + (channel * 4); - servo_device_.write_byte(base_reg, on_value & 0xFF); - servo_device_.write_byte(base_reg + 1, on_value >> 8); - servo_device_.write_byte(base_reg + 2, off_value & 0xFF); - servo_device_.write_byte(base_reg + 3, off_value >> 8); - } - - void JetRacer::set_motor_pwm(int channel, int value) - { - value = std::clamp(value, 0, 4095); - int base_reg = 0x06 + (channel * 4); - motor_device_.write_byte(base_reg, 0); - motor_device_.write_byte(base_reg + 1, 0); - motor_device_.write_byte(base_reg + 2, value & 0xFF); - motor_device_.write_byte(base_reg + 3, value >> 8); - } - - void JetRacer::set_speed(float speed) - { - - // std::cout << "[DEBUG] set_speed = " << speed << std::endl; - if (speed > 30) - speed = 30; - - speed = std::clamp(speed, -100.0f, 100.0f); - int pwm_value = static_cast(std::abs(speed) / 100.0f * 4095); - - if (speed > 0) - { - set_motor_pwm(0, pwm_value); - set_motor_pwm(1, 0); - set_motor_pwm(2, pwm_value); - set_motor_pwm(5, pwm_value); - set_motor_pwm(6, 0); - set_motor_pwm(7, pwm_value); - } - else if (speed < 0) - { - set_motor_pwm(0, pwm_value); - set_motor_pwm(1, pwm_value); - set_motor_pwm(2, 0); - set_motor_pwm(6, pwm_value); - set_motor_pwm(7, pwm_value); - set_motor_pwm(8, 0); - } - else - { - for (int channel = 0; channel < 9; ++channel) - { - set_motor_pwm(channel, 0); - } - } - - current_speed_ = speed; - } - - void JetRacer::process_joystick() - { - if (SDL_Init(SDL_INIT_JOYSTICK) < 0) - { - std::cerr << "Failed to initialize SDL: " << SDL_GetError() << std::endl; - return; - } - - SDL_Joystick *joystick = SDL_JoystickOpen(0); - if (!joystick) - { - std::cerr << "Failed to open joystick: " << SDL_GetError() << std::endl; - SDL_Quit(); - return; - } - - while (running_) - { - SDL_JoystickUpdate(); - - int left_joystick_y = SDL_JoystickGetAxis(joystick, 1); // Speed control - // int right_joystick_x = SDL_JoystickGetAxis(joystick, 2); // Directional control - - set_speed(-left_joystick_y / 32767.0f * 100); - // smooth_steering(right_joystick_x / 32767.0f * MAX_ANGLE_, 10); - - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - - SDL_JoystickClose(joystick); - SDL_Quit(); - } - - void JetRacer::start() - { - running_ = true; - std::thread joystick_thread(&JetRacer::process_joystick, this); - joystick_thread.detach(); - } - - void JetRacer::stop() - { - running_ = false; - set_speed(0); - set_steering(0); - } - - bool JetRacer::is_running() const - { - return running_.load(); - } - -} // namespace jetracer::control