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

添加了pid以及gui

Browse Source
This commit is contained in:
Liu Lida
2025-12-05 14:22:47 +08:00
parent 8f5ced6be3
commit ac566a0cf6
4 changed files with 230 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
src/MindVisionMain.cpp
View File

@@ -18,6 +18,40 @@
#include "Visualizer.h"
#include "BallisticPredictor.h"
#include "TTLCommunicator.h"
#include "PidController.h"
// Global PID controllers for pitch and yaw
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
void on_pitch_kp_trackbar(int pos, void*) {
g_pitch_pid->setKp(pos / 100.0f);
}
void on_pitch_ki_trackbar(int pos, void*) {
g_pitch_pid->setKi(pos / 1000.0f);
}
void on_pitch_kd_trackbar(int pos, void*) {
g_pitch_pid->setKd(pos / 100.0f);
}
void on_yaw_kp_trackbar(int pos, void*) {
g_yaw_pid->setKp(pos / 100.0f);
}
void on_yaw_ki_trackbar(int pos, void*) {
g_yaw_pid->setKi(pos / 1000.0f);
}
void on_yaw_kd_trackbar(int pos, void*) {
g_yaw_pid->setKd(pos / 100.0f);
}
// Function to output control data to TTL device (with enable control)
void output_control_data(const cv::Point2f* ballistic_point,
@@ -30,13 +64,34 @@ void output_control_data(const cv::Point2f* ballistic_point,
std::ostringstream send_str;
// Calculate offset (based on actual image center)
int ballistic_offset_yaw = 1.9*-static_cast<int>(ballistic_point->x - img_center.x);
if ( abs(ballistic_offset_yaw) > 320){
ballistic_offset_yaw = ( ballistic_offset_yaw / abs( ballistic_offset_yaw ) ) * 220 ;
float raw_offset_x = -(ballistic_point->x - img_center.x); // yaw error
float raw_offset_y = -(img_center.y - ballistic_point->y); // pitch error
// Calculate time delta (assuming we have access to time)
static auto last_time = std::chrono::high_resolution_clock::now();
auto current_time = std::chrono::high_resolution_clock::now();
float dt = std::chrono::duration<float>(current_time - last_time).count();
if (dt < 0.001f) dt = 0.01f; // Minimum dt to avoid division by zero
last_time = current_time;
// Apply PID control to the pitch (vertical) component
float pid_pitch_output = pitch_pid.update(0.0f, raw_offset_y, dt); // Setpoint is 0, error is raw_offset_y
// Apply PID control to the yaw (horizontal) component
float pid_yaw_output = yaw_pid.update(0.0f, raw_offset_x, dt); // Setpoint is 0, error is raw_offset_x
// 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);
// Apply same limits as before
if (abs(ballistic_offset_yaw) > 320) {
ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor
}
int ballistic_offset_pitch = 1.9*-static_cast<int>(img_center.y - ballistic_point->y);
if ( abs(ballistic_offset_pitch) > 180 ) {
ballistic_offset_pitch = ( ballistic_offset_pitch / abs( ballistic_offset_pitch ) ) * 180*1.9 ;
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;
}
// Color simplification mapping
@@ -297,10 +352,44 @@ int main(int /*argc*/, char* /*argv*/[]) {
// Now sending on every frame for smoother control
output_control_data(display_center, target_color, ttl, img_center, use_ttl);
// Create trackbars for PID parameter tuning
static bool initialized_trackbars = false;
if (!initialized_trackbars) {
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);
// 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);
// Set initial positions
cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(pitch_pid.getKp() * 100));
cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(pitch_pid.getKi() * 1000));
cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(pitch_pid.getKd() * 100));
cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(yaw_pid.getKp() * 100));
cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(yaw_pid.getKi() * 1000));
cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(yaw_pid.getKd() * 100));
initialized_trackbars = true;
}
// Update trackbar positions in case they were changed externally
cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(pitch_pid.getKp() * 100));
cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(pitch_pid.getKi() * 1000));
cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(pitch_pid.getKd() * 100));
cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(yaw_pid.getKp() * 100));
cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(yaw_pid.getKi() * 1000));
cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(yaw_pid.getKd() * 100));
// Display windows
cv::imshow("Armor Detection", frame);
cv::imshow(target_color + " Mask", mask);
cv::imshow(target_color + " Only", color_only_frame);
cv::imshow("Armor Detection", frame);
// Exit on 'q' key press
if (cv::waitKey(1) == 'q') {