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

修改报错

Browse Source
This commit is contained in:
Liu Lida
2025-12-05 18:03:03 +08:00
parent 7c77c55031
commit 1f0a1e3a7c
1 changed files with 36 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
src/MindVisionMain.cpp
View File

@@ -20,33 +20,40 @@
#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
// Function to safely access PID controllers (avoiding static initialization order issues)
PidController& getPitchPid() {
static PidController pitch_pid(0.1f, 0.01f, 0.05f);
return pitch_pid;
}
// Callback functions for trackbars - use direct access to global objects
PidController& getYawPid() {
static PidController yaw_pid(0.1f, 0.01f, 0.05f);
return yaw_pid;
}
// Callback functions for trackbars - use safe access to PID controllers
void on_pitch_kp_trackbar(int pos, void*) {
pitch_pid.setKp(pos / 100.0f);
getPitchPid().setKp(pos / 100.0f);
}
void on_pitch_ki_trackbar(int pos, void*) {
pitch_pid.setKi(pos / 1000.0f);
getPitchPid().setKi(pos / 1000.0f);
}
void on_pitch_kd_trackbar(int pos, void*) {
pitch_pid.setKd(pos / 100.0f);
getPitchPid().setKd(pos / 100.0f);
}
void on_yaw_kp_trackbar(int pos, void*) {
yaw_pid.setKp(pos / 100.0f);
getYawPid().setKp(pos / 100.0f);
}
void on_yaw_ki_trackbar(int pos, void*) {
yaw_pid.setKi(pos / 1000.0f);
getYawPid().setKi(pos / 1000.0f);
}
void on_yaw_kd_trackbar(int pos, void*) {
yaw_pid.setKd(pos / 100.0f);
getYawPid().setKd(pos / 100.0f);
}
// Function to output control data to TTL device (with enable control)
@@ -71,23 +78,31 @@ void output_control_data(const cv::Point2f* ballistic_point,
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
float pid_pitch_output = getPitchPid().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
float pid_yaw_output = getYawPid().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
// Apply same limits as before with division by zero check
if (abs(ballistic_offset_yaw) > 320) {
ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor
if (ballistic_offset_yaw != 0) {
ballistic_offset_yaw = (ballistic_offset_yaw / abs(ballistic_offset_yaw)) * 220; // Keep the scale factor
} else {
ballistic_offset_yaw = 220; // or some default value
}
}
if (abs(ballistic_offset_pitch) > 180) {
// Use the same scale factor as before
ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * ;
if (ballistic_offset_pitch != 0) {
ballistic_offset_pitch = (ballistic_offset_pitch / abs(ballistic_offset_pitch)) * 180 * 1.9;
} else {
ballistic_offset_pitch = 180 * 1.9; // or some default value
}
}
// Color simplification mapping
@@ -364,12 +379,12 @@ int main(int /*argc*/, char* /*argv*/[]) {
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));
cv::setTrackbarPos("Pitch Kp", "PID Tuning", static_cast<int>(getPitchPid().getKp() * 100));
cv::setTrackbarPos("Pitch Ki", "PID Tuning", static_cast<int>(getPitchPid().getKi() * 1000));
cv::setTrackbarPos("Pitch Kd", "PID Tuning", static_cast<int>(getPitchPid().getKd() * 100));
cv::setTrackbarPos("Yaw Kp", "PID Tuning", static_cast<int>(getYawPid().getKp() * 100));
cv::setTrackbarPos("Yaw Ki", "PID Tuning", static_cast<int>(getYawPid().getKi() * 1000));
cv::setTrackbarPos("Yaw Kd", "PID Tuning", static_cast<int>(getYawPid().getKd() * 100));
initialized_trackbars = true;
}