diff --git a/src/MindVisionMain.cpp b/src/MindVisionMain.cpp index 6bf0198..612fd6c 100644 --- a/src/MindVisionMain.cpp +++ b/src/MindVisionMain.cpp @@ -24,33 +24,29 @@ PidController pitch_pid(0.1f, 0.01f, 0.05f); // Default PID values for pitch PidController yaw_pid(0.1f, 0.01f, 0.05f); // Default PID values for yaw -// Global pointers to PID controllers to be accessed by trackbar callbacks -PidController* g_pitch_pid = &pitch_pid; -PidController* g_yaw_pid = &yaw_pid; - -// Callback functions for trackbars +// Callback functions for trackbars - use direct access to global objects void on_pitch_kp_trackbar(int pos, void*) { - g_pitch_pid->setKp(pos / 100.0f); + pitch_pid.setKp(pos / 100.0f); } void on_pitch_ki_trackbar(int pos, void*) { - g_pitch_pid->setKi(pos / 1000.0f); + pitch_pid.setKi(pos / 1000.0f); } void on_pitch_kd_trackbar(int pos, void*) { - g_pitch_pid->setKd(pos / 100.0f); + pitch_pid.setKd(pos / 100.0f); } void on_yaw_kp_trackbar(int pos, void*) { - g_yaw_pid->setKp(pos / 100.0f); + yaw_pid.setKp(pos / 100.0f); } void on_yaw_ki_trackbar(int pos, void*) { - g_yaw_pid->setKi(pos / 1000.0f); + yaw_pid.setKi(pos / 1000.0f); } void on_yaw_kd_trackbar(int pos, void*) { - g_yaw_pid->setKd(pos / 100.0f); + yaw_pid.setKd(pos / 100.0f); } // Function to output control data to TTL device (with enable control) @@ -71,7 +67,7 @@ void output_control_data(const cv::Point2f* ballistic_point, static auto last_time = std::chrono::high_resolution_clock::now(); auto current_time = std::chrono::high_resolution_clock::now(); float dt = std::chrono::duration(current_time - last_time).count(); - if (dt < 0.001f) dt = 0.01f; // Minimum dt to avoid division by zero + if (dt <= 0) dt = 0.01f; // Minimum dt to avoid division by zero last_time = current_time; // Apply PID control to the pitch (vertical) component @@ -82,16 +78,16 @@ void output_control_data(const cv::Point2f* ballistic_point, // Convert PID outputs to the expected format // The PID output might be large, so we might need to scale it - int ballistic_offset_yaw = 1.9 * (-static_cast(pid_yaw_output)); - int ballistic_offset_pitch = 1.9 * (-static_cast(pid_pitch_output)); + int ballistic_offset_yaw = static_cast(pid_yaw_output); + int ballistic_offset_pitch = static_cast(pid_pitch_output); // Apply same limits as before if (abs(ballistic_offset_yaw) > 320) { - ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220*1.9; // Keep the scale factor + ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor } if (abs(ballistic_offset_pitch) > 180) { // Use the same scale factor as before - ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * 1.9; + ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * ; } // Color simplification mapping @@ -126,8 +122,8 @@ int main(int /*argc*/, char* /*argv*/[]) { static int Numbe = 0; std::string target_color = "red"; int cam_id = 0; - cv::Size default_resolution(1280, 720); // Changed to 640x480 for consistency with SJTU project - bool use_ttl = true; // Set to false to disable TTL communication + cv::Size default_resolution(640, 480); // Changed to 640x480 for consistency with SJTU project + bool use_ttl = false; // Set to false to disable TTL communication if (Numbe == 0) { @@ -378,14 +374,6 @@ int main(int /*argc*/, char* /*argv*/[]) { initialized_trackbars = true; } - // Update trackbar positions in case they were changed externally - cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast(pitch_pid.getKp() * 100)); - cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast(pitch_pid.getKi() * 1000)); - cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast(pitch_pid.getKd() * 100)); - cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast(yaw_pid.getKp() * 100)); - cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast(yaw_pid.getKi() * 1000)); - cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast(yaw_pid.getKd() * 100)); - // Display windows cv::imshow("Armor Detection", frame); cv::imshow(target_color + " Mask", mask);