VSAG/Catalyst-MDVS
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

data had upgrade

Browse Source
This commit is contained in:
Luo Yufei
2025-12-06 14:42:59 +08:00
parent bf709ea616
commit 79654f44ff
2 changed files with 35 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
src/MindVisionMain.cpp
View File

@@ -21,12 +21,12 @@
#include "PidController.h"
// 全局PID参数（使用简单类型避免初始化问题）
float g_pitch_kp = 0.1f;
float g_pitch_ki = 0.01f;
float g_pitch_kd = 0.05f;
float g_yaw_kp = 0.1f;
float g_yaw_ki = 0.01f;
float g_yaw_kd = 0.05f;
float g_pitch_kp = 0.5217f;
float g_pitch_ki = 0.006f;
float g_pitch_kd = 0.0773f;
float g_yaw_kp = 0.6419f;
float g_yaw_ki = 0.2409f;
float g_yaw_kd = 0.4372f;
// PID控制器实例
PidController pitch_pid(g_pitch_kp, g_pitch_ki, g_pitch_kd);
@@ -45,27 +45,27 @@ void update_pid_controllers() {
// 轨迹条回调函数
void on_pitch_kp_trackbar(int pos, void*) {
g_pitch_kp = pos / 100.0f;
g_pitch_kp = pos / 10000.0f;
}
void on_pitch_ki_trackbar(int pos, void*) {
g_pitch_ki = pos / 1000.0f;
g_pitch_ki = pos / 100000.0f;
}
void on_pitch_kd_trackbar(int pos, void*) {
g_pitch_kd = pos / 100.0f;
g_pitch_kd = pos / 100000.0f;
}
void on_yaw_kp_trackbar(int pos, void*) {
g_yaw_kp = pos / 100.0f;
g_yaw_kp = pos / 10000.0f;
}
void on_yaw_ki_trackbar(int pos, void*) {
g_yaw_ki = pos / 1000.0f;
g_yaw_ki = pos / 100000.1f;
}
void on_yaw_kd_trackbar(int pos, void*) {
g_yaw_kd = pos / 100.0f;
g_yaw_kd = pos / 100000.0f;
}
// Function to output control data to TTL device (with enable control)
@@ -80,7 +80,7 @@ void output_control_data(const cv::Point2f* ballistic_point,
// Calculate offset (based on actual image center)
float raw_offset_x = -(ballistic_point->x - img_center.x); // yaw error
float raw_offset_y = -(img_center.y - ballistic_point->y); // pitch error
float raw_offset_y = -(img_center.y - ballistic_point->y - 30); // pitch error
// Calculate time delta
static auto last_time = std::chrono::high_resolution_clock::now();
@@ -100,25 +100,9 @@ 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 = static_cast<int>(pid_yaw_output);
int ballistic_offset_pitch = static_cast<int>(pid_pitch_output);
int ballistic_offset_yaw = 1.4*(-static_cast<int>(pid_yaw_output));
int ballistic_offset_pitch = 2*(-static_cast<int>(pid_pitch_output));
// Apply same limits as before with division by zero check
if (abs(ballistic_offset_yaw) > 320) {
if (ballistic_offset_yaw != 0) {
ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor
} else {
ballistic_offset_yaw = ballistic_offset_yaw > 0 ? 220 : -220; // or some default value
}
}
if (abs(ballistic_offset_pitch) > 180) {
// Use the same scale factor as before
if (ballistic_offset_pitch != 0) {
ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * 1.9;
} else {
ballistic_offset_pitch = ballistic_offset_pitch > 0 ? 180 * 1.9 : -180 * 1.9; // or some default value
}
}
// Color simplification mapping
std::string simplified_color = target_color;
@@ -152,8 +136,8 @@ int main(int /*argc*/, char* /*argv*/[]) {
static int Numbe = 0;
std::string target_color = "red";
int cam_id = 0;
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
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
if (Numbe == 0) {
@@ -397,30 +381,30 @@ int main(int /*argc*/, char* /*argv*/[]) {
cv::namedWindow("PID Tuning", cv::WINDOW_AUTOSIZE);
// Create trackbars for pitch PID parameters
cv::createTrackbar("Pitch Kp", "PID Tuning", nullptr, 1000, on_pitch_kp_trackbar);
cv::createTrackbar("Pitch Ki", "PID Tuning", nullptr, 1000, on_pitch_ki_trackbar);
cv::createTrackbar("Pitch Kd", "PID Tuning", nullptr, 1000, on_pitch_kd_trackbar);
cv::createTrackbar("Pitch Kp", "PID Tuning", nullptr, 10000, on_pitch_kp_trackbar);
cv::createTrackbar("Pitch Ki", "PID Tuning", nullptr, 10000, on_pitch_ki_trackbar);
cv::createTrackbar("Pitch Kd", "PID Tuning", nullptr, 10000, on_pitch_kd_trackbar);
// Create trackbars for yaw PID parameters
cv::createTrackbar("Yaw Kp", "PID Tuning", nullptr, 1000, on_yaw_kp_trackbar);
cv::createTrackbar("Yaw Ki", "PID Tuning", nullptr, 1000, on_yaw_ki_trackbar);
cv::createTrackbar("Yaw Kd", "PID Tuning", nullptr, 1000, on_yaw_kd_trackbar);
cv::createTrackbar("Yaw Kp", "PID Tuning", nullptr, 10000, on_yaw_kp_trackbar);
cv::createTrackbar("Yaw Ki", "PID Tuning", nullptr, 10000, on_yaw_ki_trackbar);
cv::createTrackbar("Yaw Kd", "PID Tuning", nullptr, 10000, on_yaw_kd_trackbar);
// Set initial positions
cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(g_pitch_kp * 100));
cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(g_pitch_ki * 1000));
cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(g_pitch_kd * 100));
cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(g_yaw_kp * 100));
cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(g_yaw_ki * 1000));
cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(g_yaw_kd * 100));
cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(g_pitch_kp * 10000));
cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(g_pitch_ki * 10000));
cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(g_pitch_kd * 10000));
cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(g_yaw_kp * 10000));
cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(g_yaw_ki * 10000));
cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(g_yaw_kd * 10000));
initialized_trackbars = true;
}
// Display windows
cv::imshow("Armor Detection", frame);
cv::imshow(target_color + " Mask", mask);
cv::imshow(target_color + " Only", color_only_frame);
//cv::imshow(target_color + " Mask", mask);
//cv::imshow(target_color + " Only", color_only_frame);
// Only call cv::waitKey to keep GUI responsive
cv::waitKey(1);

2
src/PidController.cpp
View File

@@ -4,7 +4,7 @@
PidController::PidController(float kp, float ki, float kd)
: kp_(kp), ki_(ki), kd_(kd), last_error_(0.0f), integral_(0.0f), output_(0.0f),
output_min_(-100.0f), output_max_(100.0f), first_iteration_(true) {}
output_min_(-400.0f), output_max_(400.0f), first_iteration_(true) {}
float PidController::update(float setpoint, float measured_value, float dt) {
float error = setpoint - measured_value;