diff --git a/CMakeLists.txt b/CMakeLists.txt index 353645f..eefd407 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,9 @@ find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(ament_cmake REQUIRED) +# НОВОЕ: Добавляем поиск библиотеки SDL2 +find_package(SDL2 REQUIRED) + # include_directories(/opt/ros/humble/include) include_directories("${PROJECT_SOURCE_DIR}/compass") @@ -126,6 +129,7 @@ endif() ament_target_dependencies(UMAS_GUI rclcpp geometry_msgs) +# НОВОЕ: Добавляем SDL2 в список линкуемых библиотек target_link_libraries(UMAS_GUI sfml-graphics sfml-window sfml-system Qt${QT_VERSION_MAJOR}::Widgets @@ -138,6 +142,7 @@ target_link_libraries(UMAS_GUI # Qt${QT_VERSION_MAJOR}::WebEngineWidgets # Qt${QT_VERSION_MAJOR}::Positioning Python3::Python + ${SDL2_LIBRARY} # НОВОЕ: Подключаем SDL2 библиотеку ) diff --git a/mainwindow.cpp b/mainwindow.cpp index 66a8bb3..00a8691 100644 --- a/mainwindow.cpp +++ b/mainwindow.cpp @@ -19,17 +19,39 @@ MainWindow::MainWindow(QWidget *parent) setTab(); setUpdateUI(); + // Инициализация спинбоксов + gainSpinBoxes = { + ui->spinBox_gain_surge, + ui->spinBox_gain_sway, + ui->spinBox_gain_depth, + ui->spinBox_gain_yaw, + ui->spinBox_gain_pitch, + ui->spinBox_gain_roll + }; + + qDebug() << "Количество спинбоксов:" << gainSpinBoxes.size(); + + // Загрузка сохраненных значений для ВСЕХ режимов + loadSettings(); + qDebug() << "Текущий режим после загрузки:" << currentMode; + + setSpinBoxValuesForCurrentMode(); + // Подключаем сигналы + for (auto spinBox : gainSpinBoxes) { + connect(spinBox, QOverload::of(&QSpinBox::valueChanged), + this, &MainWindow::onGainValueChanged); + } + // К каждому спинбоксу привязывается сигнал onGainValueChanged() } - void MainWindow::setWidget() { powerSystem = new PowerSystem(this); ui->horizontalLayout_for_powerSystem->addWidget(powerSystem); checkMsg = new CheckMsg(this); ui->horizontalLayout_for_checkMsg->addWidget(checkMsg); - // checkImu = new CheckImu(this); + // checkImu = new CheckImu(this); // ui->horizontalLayout_for_checkImu->addWidget(checkImu); modeAutomatic = new ModeAutomatic(this); ui->verticalLayout_modeAutomatic->addWidget(modeAutomatic); @@ -113,35 +135,33 @@ void MainWindow::setWidget() // modeAutomatic->ui->lineEdit_missionPlanning_go_circle_radius, &QLineEdit::textChanged, // mapWidget, &MapWidget::setRadius_circle); // mapWidget->setRadius_circle(modeAutomatic->ui->lineEdit_missionPlanning_go_circle_radius->text()); - - } void MainWindow::setGUI_reper() { - connect( - ui->pushButton_sendReper, &QPushButton::clicked, - this, &MainWindow::slot_pushButton_sendReper); + connect(ui->pushButton_sendReper, &QPushButton::clicked, + this, &MainWindow::slot_pushButton_sendReper); + // connect( // this, &MainWindow::signal_setMarker, // mapWidget, &MapWidget::setMarker); // connect( // mapWidget, &MapWidget::signal_addMarker_to_gui, // this, &MainWindow::slot_addMarker_to_gui); - } void MainWindow::slot_pushButton_sendReper() { - qDebug() << "hello"; - // Получение текста из QLineEdit - QString latitudeStr = ui->lineEdit_reper_latitude->text(); // Широта - QString longitudeStr = ui->lineEdit_reper_longitude->text(); // Долгота +qDebug() << "hello"; +// Получение текста из QLineEdit +QString latitudeStr = ui->lineEdit_reper_latitude->text(); // Широта +QString longitudeStr = ui->lineEdit_reper_longitude->text(); // Долгота + +// Преобразование текста в double +double latitude = latitudeStr.toDouble(); +double longitude = longitudeStr.toDouble(); - // Преобразование текста в double - double latitude = latitudeStr.toDouble(); - double longitude = longitudeStr.toDouble(); // Создание объекта координат // QGeoCoordinate reperCoordinate(latitude, longitude); @@ -150,6 +170,7 @@ void MainWindow::slot_pushButton_sendReper() // emit signal_setMarker(reperCoordinate); // Отправка репера на борт + CoordinatePoint msg; msg.x_point = latitude; msg.y_point = longitude; @@ -161,34 +182,30 @@ void MainWindow::slot_addMarker_to_gui(double latitude, double longitude) ui->lineEdit_reper_latitude->setText(QString::number(latitude, 'd', 14)); // Широта ui->lineEdit_reper_longitude->setText(QString::number(longitude, 'd', 14)); // Долгота + CoordinatePoint msg; msg.x_point = latitude; msg.y_point = longitude; uv_interface.setReper(msg); } - void MainWindow::setInterface() { - connect( - this, &MainWindow::signal_setInterface, - powerSystem, &PowerSystem::slot_getInterface); - connect( - this, &MainWindow::signal_setInterface, - checkMsg, &CheckMsg::slot_getInterface); - connect( - this, &MainWindow::signal_setInterface, - modeAutomatic, &ModeAutomatic::slot_getInterface); - + connect(this, &MainWindow::signal_setInterface, + powerSystem, &PowerSystem::slot_getInterface); + connect(this, &MainWindow::signal_setInterface, + checkMsg, &CheckMsg::slot_getInterface); + connect(this, &MainWindow::signal_setInterface, + modeAutomatic, &ModeAutomatic::slot_getInterface); + signal_setInterface(&uv_interface); - } void MainWindow::setConsole() { displayText("Приложение работает"); displayText("Установите соединение для работы с агентом"); - + connect(&uv_interface, SIGNAL(displayText_toConsole(QString)), this, SLOT(displayText(QString))); } @@ -200,29 +217,55 @@ void MainWindow::displayText(QString str) ui->textEdit_console->append(currentTime + " " + str); } -// - void MainWindow::setTimer_updateImpact(int periodUpdateMsec) { joyStick = new JoyStick(this); + keyBoard = nullptr; + + // Подключаем сигналы кнопок джойстика + connect(joyStick, &JoyStick::dPadLeftPressed, this, &MainWindow::setSpeedModeLeft); + connect(joyStick, &JoyStick::dPadRightPressed, this, &MainWindow::setSpeedModeRight); + connect(joyStick, &JoyStick::backButtonPressed, this, &MainWindow::useKeyBoard); + connect(ui->radioButton_useJoyStick, &QRadioButton::clicked, this, &MainWindow::useJoyStick); connect(ui->radioButton_useKeyBoard, &QRadioButton::clicked, this, &MainWindow::useKeyBoard); + + ui->radioButton_useJoyStick->setChecked(true); + ui->radioButton_useKeyBoard->setChecked(false); + QTimer *updateTimer = new QTimer(this); - connect( - updateTimer, SIGNAL(timeout()), - this, SLOT(updateUi_fromControl()) - ); + connect(updateTimer, SIGNAL(timeout()), + this, SLOT(updateUi_fromControl())); updateTimer->start(periodUpdateMsec); + displayText("Таймер обновления джойстика запущен"); } void MainWindow::useKeyBoard() { - delete joyStick; - - keyBoard = new KeyBoard(this); + qDebug() << "Переключаюсь на клавиатуру..."; + + // Сначала создаем клавиатуру, если ее нет + if (!keyBoard) { + keyBoard = new KeyBoard(this); + } + + // Удаляем джойстик + if (joyStick) { + disconnect(joyStick, &JoyStick::buttonXPressed, this, &MainWindow::setSpeedModeLeft); + disconnect(joyStick, &JoyStick::buttonBPressed, this, &MainWindow::setSpeedModeRight); + disconnect(joyStick, &JoyStick::backButtonPressed, this, &MainWindow::useKeyBoard); + + joyStick->deleteLater(); + joyStick = nullptr; + } + + // Обновляем UI + ui->radioButton_useKeyBoard->setChecked(true); + ui->radioButton_useJoyStick->setChecked(false); + displayText("Используемые клавиши(должна быть английская раскладка):\n" "Клавиша O - вперед по маршу\n" "Клавиша L - назад по маршу\n" @@ -236,63 +279,235 @@ void MainWindow::useKeyBoard() "Клавиша E - вправо по крену\n" "Клавиша K - влево по лагу\n" "Клавиша ; - вправо по лагу\n"); - } void MainWindow::useJoyStick() { - delete keyBoard; - + if (keyBoard) { + delete keyBoard; + keyBoard = nullptr; + } + joyStick = new JoyStick(this); + + // Подключаем сигналы кнопок джойстика + + connect(joyStick, &JoyStick::backButtonPressed, this, &MainWindow::useKeyBoard); + + + connect(joyStick, &JoyStick::dPadLeftPressed, this, &MainWindow::setSpeedModeLeft); + connect(joyStick, &JoyStick::dPadRightPressed, this, &MainWindow::setSpeedModeRight); + + ui->radioButton_useJoyStick->setChecked(true); + ui->radioButton_useKeyBoard->setChecked(false); + + displayText("Переключено на управление геймпадом"); +} + +void MainWindow::setSpeedModeLeft() { + qDebug() << "setSpeedModeLeft called, currentMode =" << currentMode; + + // SLOW -> MEDIUM -> FAST -> SLOW + switch (currentMode) { + case SLOW: + setSpeedModeMedium(); + break; + case MEDIUM: + setSpeedModeFast(); + break; + case FAST: + setSpeedModeSlow(); + break; + } +} + +void MainWindow::setSpeedModeRight() { + qDebug() << "setSpeedModeRight called, currentMode =" << currentMode; + + // SLOW <- MEDIUM <- FAST <- SLOW + switch (currentMode) { + case SLOW: + setSpeedModeFast(); + break; + case MEDIUM: + setSpeedModeSlow(); + break; + case FAST: + setSpeedModeMedium(); + break; + } } void MainWindow::keyPressEvent(QKeyEvent *event) { - keyBoard->keyPressEvent(event); + if (keyBoard) { + keyBoard->keyPressEvent(event); + } } void MainWindow::keyReleaseEvent(QKeyEvent *event) { - keyBoard->keyReleaseEvent(event); + if (keyBoard) { + keyBoard->keyReleaseEvent(event); + } } -void MainWindow::updateUi_fromControl(){ - - ControlData control = uv_interface.getControlData(); - DataAH127C imuData = uv_interface.getImuData(); - - ui->compass->setYawDesirable(control.yaw, imuData.yaw, uv_interface.getCSMode()); - - ui->label_controlYaw->setNum(control.yaw * ui->spinBox_gain_yaw->value()); - ui->label_controlMarch->setNum(control.march * ui->spinBox_gain_surge->value()); - ui->label_controlDif->setNum(control.pitch * ui->spinBox_gain_pitch->value()); - ui->label_controlLag->setNum(control.lag * ui->spinBox_gain_sway->value()); - ui->label_controlDepth->setNum(control.depth * ui->spinBox_gain_depth->value()); - ui->label_controlKren->setNum(control.roll * ui->spinBox_gain_roll->value()); - rosBridge->publishCommand(control.march * ui->spinBox_gain_surge->value(), control.lag * ui->spinBox_gain_sway->value(), control.depth * ui->spinBox_gain_depth->value(), control.roll * ui->spinBox_gain_roll->value(), control.pitch * ui->spinBox_gain_pitch->value(), control.yaw * ui->spinBox_gain_yaw->value()); +void MainWindow::updateUi_fromControl() +{ + ControlData control = uv_interface.getControlData(); + DataAH127C imuData = uv_interface.getImuData(); + + // Инициализируем все оси управления нулями + control.march = 0.0f; + control.yaw = 0.0f; + control.depth = 0.0f; + control.roll = 0.0f; + control.pitch = 0.0f; + control.lag = 0.0f; + + if (joyStick && joyStick->isConnected()) { + // --- ЛЕВЫЙ ДЖОЙСТИК: марш и курс --- + QPair leftStickValues = joyStick->getLeftStickValues(); + float leftStickX = leftStickValues.first; // Вбок (курс) + float leftStickY = leftStickValues.second; // Вперед/назад (марш) + + // --- ПРАВЫЙ ДЖОЙСТИК --- + QPair rightStickValues = joyStick->getRightStickValues(); + float rightStickX = rightStickValues.first; // Roll (крен) + float rightStickY = rightStickValues.second; // Pitch (дифферент) + + // --- ДЖОЙСТИК ДИФФЕРЕНТ --- + QPair triggerValues = joyStick->getTriggerValues(); + float leftTrigger = triggerValues.first; // L2 (0-100) + float rightTrigger = triggerValues.second; // R2 (0-100) + + // --- КАЛИБРОВКА ПРАВОГО ДЖОЙСТИКА (при первом подключении) --- + static bool isRightStickCalibrated = false; + static float rightStickZeroX = 0.0f; + static float rightStickZeroY = 0.0f; + + if (!isRightStickCalibrated) { + rightStickZeroX = rightStickX; + rightStickZeroY = rightStickY; + isRightStickCalibrated = true; + qDebug() << "Калибровка правого джойстика: X0 =" << rightStickZeroX << "Y0 =" << rightStickZeroY; + } + + // Вычитаем нулевые значения + rightStickX -= rightStickZeroX; + rightStickY -= rightStickZeroY; + + + const float leftDeadZone = 10.0f; + const float rightDeadZone = 0; + const float triggerDeadZone = 20.0f; + + + if (fabs(leftStickY) > leftDeadZone) { + control.march = leftStickY / 100.0f; + } + + if (fabs(leftStickX) > leftDeadZone) { + control.yaw = leftStickX / 100.0f; + } + + // --- УПРАВЛЕНИЕ ДИФФЕРЕНТОМ ПРАВЫМ ДЖОЙСТИКОМ--- + float pitchControl = 0.0f; + + + if (leftTrigger > triggerDeadZone) { + pitchControl = leftTrigger / 100.0f; + } + + + if (rightTrigger > triggerDeadZone) { + pitchControl = -rightTrigger / 100.0f; + } + + + if (leftTrigger > triggerDeadZone && rightTrigger > triggerDeadZone) { + pitchControl = (leftTrigger - rightTrigger) / 100.0f; + } + + control.pitch = pitchControl; + + + + + // --- тригерами L2 R2 ЭТИ КОММЕНТАРИИ ВЕРНЫ ГЛУБИНА --- + + if (fabs(rightStickY) > rightDeadZone) { + + control.depth = -rightStickY / 100.0f; + qDebug() << "!!!!"; + } + + if (fabs(rightStickX) > rightDeadZone) { + control.depth = rightStickX / 100.0f; + qDebug() << "------!!!!"; + } + + + } + + // Обновляем компас с текущими значениями управления + ui->compass->setYawDesirable(control.yaw, imuData.yaw, uv_interface.getCSMode()); + + // Обновляем значения на интерфейсе + ui->label_controlYaw->setNum(control.yaw * ui->spinBox_gain_yaw->value()); + ui->label_controlMarch->setNum(control.march * ui->spinBox_gain_surge->value()); + ui->label_controlDif->setNum(control.pitch * ui->spinBox_gain_pitch->value()); + ui->label_controlLag->setNum(control.lag * ui->spinBox_gain_sway->value()); + ui->label_controlDepth->setNum(control.depth * ui->spinBox_gain_depth->value()); + ui->label_controlKren->setNum(control.roll * ui->spinBox_gain_roll->value()); + + // Отправляем команды через ROS Bridge + rosBridge->publishCommand( + control.march * ui->spinBox_gain_surge->value(), + control.lag * ui->spinBox_gain_sway->value(), + control.depth * ui->spinBox_gain_depth->value(), + control.roll * ui->spinBox_gain_roll->value(), + control.pitch * ui->spinBox_gain_pitch->value(), + control.yaw * ui->spinBox_gain_yaw->value() + ); } + void MainWindow::updateUi_Map() { GPS_coordinate gps_coordinate = uv_interface.getCoordinateGPS(); - emit signal_sendCurrentPos(gps_coordinate.latitude, gps_coordinate.longitude); // Отправка сигнала с текущими координатами - + emit signal_sendCurrentPos(gps_coordinate.latitude, gps_coordinate.longitude); // Отправка сигнала с текущими координатами } -// - void MainWindow::setBottom() { setBottom_mode(); setBottom_connection(); setBottom_modeSelection(); setBottom_selectAgent(); - + connect(ui->pushButton_zeroYaw, - &QPushButton::clicked, - rosBridge, - &RosBridge::zeroYaw); - + &QPushButton::clicked, + rosBridge, + &RosBridge::zeroYaw); + + // Инициализация трех кнопок для режимов + if (ui->pushButton_speedFast_2) { + connect(ui->pushButton_speedFast_2, &QPushButton::clicked, + this, &MainWindow::setSpeedModeFast); + } + + if (ui->pushButton_speedMedium_2) { + connect(ui->pushButton_speedMedium_2, &QPushButton::clicked, + this, &MainWindow::setSpeedModeMedium); + } + + if (ui->pushButton_speedSlow_2) { + connect(ui->pushButton_speedSlow_2, &QPushButton::clicked, + this, &MainWindow::setSpeedModeSlow); + } + + ui->pushButton_speedMedium_2->setStyleSheet("background-color: purple; font-size: 25px"); } void MainWindow::setBottom_mode() @@ -305,9 +520,9 @@ void MainWindow::setBottom_mode() mode->addButton(ui->pushButton_modeAutomated); mode->addButton(ui->pushButton_modeAutomatic); mode->setExclusive(true); - + ui->pushButton_modeManual->setChecked(true); - + ui->pushButton_modeAutomated_surge->setCheckable(true); ui->pushButton_modeAutomated_sway->setCheckable(true); ui->pushButton_modeAutomated_depth->setCheckable(true); @@ -322,39 +537,28 @@ void MainWindow::setBottom_mode() modeAutomated->addButton(ui->pushButton_modeAutomated_pitch); modeAutomated->addButton(ui->pushButton_modeAutomated_roll); modeAutomated->setExclusive(false); - - connect(ui->pushButton_modeAutomated_surge, &QPushButton::toggled, - rosBridge, &RosBridge::setModeSurge); - - connect(ui->pushButton_modeAutomated_sway, &QPushButton::toggled, + + connect(ui->pushButton_modeAutomated_surge, &QPushButton::toggled, + rosBridge, &RosBridge::setModeSurge); + connect(ui->pushButton_modeAutomated_sway, &QPushButton::toggled, rosBridge, &RosBridge::setModeSway); - - connect(ui->pushButton_modeAutomated_depth, &QPushButton::toggled, + connect(ui->pushButton_modeAutomated_depth, &QPushButton::toggled, rosBridge, &RosBridge::setModeHeave); - - connect(ui->pushButton_modeAutomated_yaw, &QPushButton::toggled, + connect(ui->pushButton_modeAutomated_yaw, &QPushButton::toggled, rosBridge, &RosBridge::setModeYaw); - - connect(ui->pushButton_modeAutomated_pitch, &QPushButton::toggled, + connect(ui->pushButton_modeAutomated_pitch, &QPushButton::toggled, rosBridge, &RosBridge::setModePitch); - - connect(ui->pushButton_modeAutomated_roll, &QPushButton::toggled, + connect(ui->pushButton_modeAutomated_roll, &QPushButton::toggled, rosBridge, &RosBridge::setModeRoll); - - + e_CSModeManualToggled(); - - connect( - ui->pushButton_modeManual, SIGNAL(clicked()), - this, SLOT(e_CSModeManualToggled())); - - connect( - ui->pushButton_modeAutomated, SIGNAL(clicked()), - this, SLOT(e_CSModeAutomatedToggled())); - - connect( - ui->pushButton_modeAutomatic, SIGNAL(clicked()), - this, SLOT(e_CSModeAutomaticToggled())); + + connect(ui->pushButton_modeManual, SIGNAL(clicked()), + this, SLOT(e_CSModeManualToggled())); + connect(ui->pushButton_modeAutomated, SIGNAL(clicked()), + this, SLOT(e_CSModeAutomatedToggled())); + connect(ui->pushButton_modeAutomatic, SIGNAL(clicked()), + this, SLOT(e_CSModeAutomaticToggled())); } void MainWindow::e_CSModeManualToggled() { @@ -371,17 +575,14 @@ void MainWindow::e_CSModeAutomaticToggled() ui->stackedWidget_mode->setCurrentIndex(1); } - void MainWindow::setBottom_connection() { ui->pushButton_breakConnection->setEnabled(false); - - connect( - ui->pushButton_connection, SIGNAL(clicked()), - this, SLOT(setConnection())); - connect( - ui->pushButton_breakConnection, SIGNAL(clicked()), - this, SLOT(breakConnection())); + + connect(ui->pushButton_connection, SIGNAL(clicked()), + this, SLOT(setConnection())); + connect(ui->pushButton_breakConnection, SIGNAL(clicked()), + this, SLOT(breakConnection())); } void MainWindow::setConnection() @@ -393,7 +594,7 @@ void MainWindow::setConnection() communicationAgent1 = new Pult::PC_Protocol(QHostAddress(ip_pult), 13053, QHostAddress(ip_agent), 13050, 10, 0); communicationAgent1->startExchange(); - + if (communicationAgent1->bindState()) { displayText("Соединение установлено"); ui->pushButton_selectAgent1->setStyleSheet("background-color: green"); @@ -410,7 +611,7 @@ void MainWindow::setConnection() void MainWindow::updateUi_fromAgent1() { DataAH127C imuData = uv_interface.getImuData(); - + emit updateCompass(imuData.yaw); emit updateIMU(imuData); emit updateSetupMsg(); @@ -418,32 +619,27 @@ void MainWindow::updateUi_fromAgent1() { emit updateMap(); } - void MainWindow::breakConnection() { ui->pushButton_connection->setEnabled(true); - + displayText("Соединение разорвано"); communicationAgent1->stopExhange(); - + delete communicationAgent1; - + ui->pushButton_breakConnection->setEnabled(false); } -// - void MainWindow::setBottom_modeSelection() { setModeSelection(0); - connect( - ui->comboBox_modeSelection, SIGNAL(activated(int)), - this, SLOT(setModeSelection(int))); + connect(ui->comboBox_modeSelection, SIGNAL(activated(int)), + this, SLOT(setModeSelection(int))); } void MainWindow::setModeSelection(int index) { - if (index == 1){ uv_interface.setModeSelection(0); //агент } @@ -452,20 +648,15 @@ void MainWindow::setModeSelection(int index) } } - -// - - - void MainWindow::setBottom_selectAgent() { ui->pushButton_selectAgent1->setCheckable(true); QButtonGroup *buttonGroup_selectAgent = new QButtonGroup(this); buttonGroup_selectAgent->addButton(ui->pushButton_selectAgent1); buttonGroup_selectAgent->setExclusive(true); - + ui->pushButton_selectAgent1->setChecked(true); - + connect(ui->pushButton_selectAgent1, &QAbstractButton::toggled, this, &MainWindow::pushButton_selectAgent1); connect(this, &MainWindow::updateStatePushButton, @@ -481,7 +672,6 @@ void MainWindow::pushButton_selectAgent1(bool stateBottom) updateStatePushButton(); } - void MainWindow::updateUi_statePushButton() { // вывод данных: модель/агент @@ -489,19 +679,18 @@ void MainWindow::updateUi_statePushButton() ui->comboBox_modeSelection->setCurrentIndex(0); else ui->comboBox_modeSelection->setCurrentIndex(1); - - + // switch (uv_interface.getCSMode()) { - case e_CSMode::MODE_MANUAL: - ui->pushButton_modeManual->setChecked(true); - break; - case e_CSMode::MODE_AUTOMATED: - ui->pushButton_modeAutomated->setChecked(true); - break; - case e_CSMode::MODE_AUTOMATIC: - ui->pushButton_modeAutomatic->setChecked(true); - break; + case e_CSMode::MODE_MANUAL: + ui->pushButton_modeManual->setChecked(true); + break; + case e_CSMode::MODE_AUTOMATED: + ui->pushButton_modeAutomated->setChecked(true); + break; + case e_CSMode::MODE_AUTOMATIC: + ui->pushButton_modeAutomatic->setChecked(true); + break; } } @@ -513,15 +702,12 @@ void MainWindow::setTab() ui->tabWidget->setTabText(3, "Режимы питания"); ui->tabWidget->setTabText(4, "Карта ГИC"); ui->tabWidget->setCurrentIndex(4); - } void MainWindow::setUpdateUI() { connect(this, SIGNAL(updateCompass(float)), this, SLOT(updateUi_Compass(float))); - // connect(this, SIGNAL(updateIMU(DataAH127C)), - // checkImu, SLOT(updateUi_imu(DataAH127C))); connect(this, SIGNAL(updateSetupMsg()), checkMsg, SLOT(updateUi_checkMsg())); connect(this, SIGNAL(updateDataMission()), @@ -530,6 +716,121 @@ void MainWindow::setUpdateUI() this, &MainWindow::updateUi_Map); } +void MainWindow::loadSettings() { + QSettings settings("/UMAS_GUI/umas_settings.ini", QSettings::IniFormat); + + // Загружаем значения для каждого режима + for (int mode = 0; mode < 3; mode++) { + settings.beginGroup(QString("SpeedMode_%1").arg(mode)); + + QMap gains; + for (int i = 0; i < gainNames.size(); i++) { + // Значения по умолчанию для каждого режима + double defaultValue = 0; + if (mode == SLOW) defaultValue = 10; + else if (mode == MEDIUM) defaultValue = 50; + else if (mode == FAST) defaultValue = 100; + + gains[gainNames[i]] = settings.value(gainNames[i], defaultValue).toDouble(); + } + + speedModeGains[mode] = gains; + settings.endGroup(); + } + + currentMode = static_cast( + settings.value("CurrentSpeedMode", MEDIUM).toInt() + ); +} + +void MainWindow::saveSettings() { + QSettings settings("/UMAS_GUI/umas_settings.ini", QSettings::IniFormat); + + // Сохраняем значения для всех режимов + for (int mode = 0; mode < 3; mode++) { + settings.beginGroup(QString("SpeedMode_%1").arg(mode)); + + for (int i = 0; i < gainNames.size(); i++) { + settings.setValue(gainNames[i], speedModeGains[mode][gainNames[i]]); + } + + settings.endGroup(); + } + + // Сохраняем текущий режим + settings.setValue("CurrentSpeedMode", currentMode); +} + +void MainWindow::saveCurrentModeGains() { + // Сохраняем текущие значения спинбоксов в текущий режим + for (int i = 0; i < gainSpinBoxes.size(); i++) { + speedModeGains[currentMode][gainNames[i]] = gainSpinBoxes[i]->value(); + } + + // Сохраняем в настройки + saveSettings(); +} + +void MainWindow::setSpinBoxValuesForCurrentMode() { + qDebug() << "setSpinBoxValuesForCurrentMode: режим =" << currentMode; + + // блокировка сигналов + for (auto spinBox : gainSpinBoxes) { + spinBox->blockSignals(true); + } + + // Устанавливаем значения из текущего режима + for (int i = 0; i < gainSpinBoxes.size(); i++) { + double value = speedModeGains[currentMode][gainNames[i]]; + qDebug() << gainNames[i] << "=" << value; + gainSpinBoxes[i]->setValue(value); + } + + // Разблокируем сигналы + for (auto spinBox : gainSpinBoxes) { + spinBox->blockSignals(false); + } +} + +void MainWindow::onGainValueChanged() { + qDebug() << "onGainValueChanged вызван"; + // Сохраняем изменения в текущем режиме + saveCurrentModeGains(); +} + +void MainWindow::setSpeedModeFast() { + qDebug() << "Устанавливаем FAST режим"; + // ВАЖНО: Сохраняем текущие значения СТАРОГО режима + saveCurrentModeGains(); + currentMode = FAST; + setSpinBoxValuesForCurrentMode(); + ui->pushButton_speedFast_2->setStyleSheet("background-color: purple;font-size: 25px"); + ui->pushButton_speedMedium_2->setStyleSheet("background-color: white;font-size: 25px"); + ui->pushButton_speedSlow_2->setStyleSheet("background-color: white;font-size: 25px"); +} + +void MainWindow::setSpeedModeMedium() { + qDebug() << "Устанавливаем MEDIUM режим"; + // ВАЖНО: Сохраняем текущие значения СТАРОГО режима + saveCurrentModeGains(); + currentMode = MEDIUM; + setSpinBoxValuesForCurrentMode(); + ui->pushButton_speedFast_2->setStyleSheet("background-color: white; font-size: 25px"); + ui->pushButton_speedMedium_2->setStyleSheet("background-color: purple; font-size: 25px"); + ui->pushButton_speedSlow_2->setStyleSheet("background-color: white; font-size: 25px"); +} + +void MainWindow::setSpeedModeSlow() { + qDebug() << "Устанавливаем SLOW режим"; + // ВАЖНО: Сохраняем текущие значения СТАРОГО режима + saveCurrentModeGains(); + currentMode = SLOW; + setSpinBoxValuesForCurrentMode(); + ui->pushButton_speedFast_2->setStyleSheet("background-color: white; font-size: 25px"); + ui->pushButton_speedMedium_2->setStyleSheet("background-color: white; font-size: 25px"); + ui->pushButton_speedSlow_2->setStyleSheet("background-color: purple; font-size: 25px"); +} + void MainWindow::updateUi_Compass(float yaw) { ui->compass->setYaw(yaw); } @@ -537,4 +838,4 @@ void MainWindow::updateUi_Compass(float yaw) { MainWindow::~MainWindow() { delete ui; -} +} \ No newline at end of file diff --git a/mainwindow.h b/mainwindow.h index d576ee5..bbe285c 100644 --- a/mainwindow.h +++ b/mainwindow.h @@ -25,6 +25,11 @@ #include "diagnostic_board.h" #include "ros2_bridge.h" +#include +// #include +// #include + + QT_BEGIN_NAMESPACE namespace Ui { class MainWindow; } @@ -52,6 +57,51 @@ class MainWindow : public QMainWindow void setConsole(); private: + + + enum SpeedMode { SLOW = 0, MEDIUM = 1, FAST = 2 }; // перечисление, для трех режимов скоростей + SpeedMode currentMode = MEDIUM; // храним текущий режим, по умолчанию среднйи + + // хранение значений: [режим][спинбокс] + QMap> speedModeGains; + + /* + map - словарь из словарей + int - первый ключ, отвечает за номер режима 0, 1, 2 + QString - второй ключ , имя спинбокса + + SpeedMode = { + 0: { // SLOW режим + "surge": 10.0, + "sway": 10.0, + "depth": 10.0, + }, + 1: { // MEDIUM режим + "surge": 50.0, + "sway": 50.0, + }, + 2: { // FAST режим + "surge": 100.0, + "sway": 100.0, + } + } + + */ + + // Массив спинбоксов + QVector gainSpinBoxes; + QStringList gainNames = {"surge", "sway", "depth", "yaw", "pitch", "roll"}; + + void saveCurrentModeGains(); // Сохраняем текущие значения спинбоксов в текущий режим + void loadCurrentModeGains(); + void setSpinBoxValuesForCurrentMode(); // начальные значения для текущего режима + + void saveSettings(); // сохранение настроек + void loadSettings(); // загрузка настроек для ВСЕХ режимов + + + + /*! * \brief setTimer_updateImpact устанавливает таймер * обработки пульта управления. @@ -104,7 +154,13 @@ class MainWindow : public QMainWindow RosBridge *rosBridge; private slots: + void onGainValueChanged(); + void setSpeedModeFast(); // слот для быстрого режима + void setSpeedModeMedium(); // слот для среднего режима + void setSpeedModeSlow(); // слот для медленного режима + void setSpeedModeLeft(); + void setSpeedModeRight(); /*! * \brief displayText слот для вывода сообщений в консоль. * \param str является выводимой строкой. @@ -173,6 +229,8 @@ private slots: void slot_addMarker_to_gui(double x, double y); + + signals: /*! * \brief updateCompass сигнал запуска обновления компаса на UI форме. diff --git a/mainwindow.ui b/mainwindow.ui index 422717d..07d6fa5 100644 --- a/mainwindow.ui +++ b/mainwindow.ui @@ -6,15 +6,15 @@ 0 0 - 3840 - 2400 + 1451 + 2076 UMAS GUI - + @@ -350,7 +350,7 @@ 0 - + @@ -379,7 +379,41 @@ - + + + + + font-size: 25px + + + Быстрый режим + + + + + + + font-size: 25px + + + Средний режим + + + + + + + font-size: 25px + + + Слабый режим + + + + + + + @@ -432,14 +466,7 @@ - - - Управляющие воздействия - - - Qt::AlignCenter - - + @@ -808,7 +835,7 @@ 0 0 - 3840 + 1451 23 diff --git a/remote_control/joy_stick.cpp b/remote_control/joy_stick.cpp index 4151f4b..498f23d 100644 --- a/remote_control/joy_stick.cpp +++ b/remote_control/joy_stick.cpp @@ -1,7 +1,6 @@ #include "joy_stick.h" -JoyStick::JoyStick(QObject *parent) -{ +JoyStick::JoyStick(QObject *parent) { id = 0; int periodUpdateMsec = 20; @@ -9,29 +8,278 @@ JoyStick::JoyStick(QObject *parent) connect(updateTimer, &QTimer::timeout, this, &JoyStick::updateImpact); updateTimer->start(periodUpdateMsec); - impactAxisMarch = sf::Joystick::Y; - impactAxisDepth = sf::Joystick::Z; - impactAxisRoll = sf::Joystick::PovX; - impactAxisPitch = sf::Joystick::PovY; - impactAxisYaw = sf::Joystick::R; - impactAxisLag = sf::Joystick::X; -} + // Для DEXP G-15 настраиваем оси + impactAxisMarch = sf::Joystick::Y; // Левый стик Y (вперед/назад) + impactAxisLag = sf::Joystick::X; // Левый стик X (влево/вправо) + impactAxisDepth = sf::Joystick::Z; // Правый стик Y + impactAxisYaw = sf::Joystick::R; // Правый стик X + impactAxisRoll = sf::Joystick::PovX; // Крестовина X + impactAxisPitch = sf::Joystick::PovY; // Крестовина Y + + // Для триггеров L2/R2 используем оси U и V + impactAxisLeftTrigger = sf::Joystick::U; // Триггер L2 (ось U) + impactAxisRightTrigger = sf::Joystick::V; // Триггер R2 (ось V) + + // Инициализация предыдущих значений осей + prevLeftStickY = 0.0f; + prevLeftStickX = 0.0f; + prevRightStickY = 0.0f; + prevRightStickX = 0.0f; + prevLeftTrigger = 0.0f; + prevRightTrigger = 0.0f; + + // Инициализация состояния кнопок + prevButtonAState = false; + prevButtonBState = false; + prevButtonXState = false; + prevButtonYState = false; + prevStartButtonState = false; + prevBackButtonState = false; + + // Для кнопок L1/R1 (обычно кнопки 4 и 5) + prevL1State = false; + prevR1State = false; -JoyStick::~JoyStick() -{ + // состояние крестика + prevDPadUp = false; + prevDPadDown = false; + prevDPadLeft = false; + prevDPadRight = false; } +JoyStick::~JoyStick() {} + void JoyStick::updateImpact() { sf::Joystick::update(); DataAH127C imuData = getImuData(); if (sf::Joystick::isConnected(id)) { - setMarch(-sf::Joystick::getAxisPosition(id, impactAxisMarch)/2); - setLag(sf::Joystick::getAxisPosition(id, impactAxisLag)/2); -// setDepth(3*sf::Joystick::getAxisPosition(id, impactAxisDepth)/4); - setRoll(sf::Joystick::getAxisPosition(id, impactAxisRoll)/4); - setPitch(sf::Joystick::getAxisPosition(id, impactAxisPitch)/10); - setYaw((sf::Joystick::getAxisPosition(id, impactAxisYaw)/4)); + // --- ОБРАБОТКА ЛЕВОГО ДЖОЙСТИКА --- + float leftStickX = sf::Joystick::getAxisPosition(id, sf::Joystick::X); + float leftStickY = sf::Joystick::getAxisPosition(id, sf::Joystick::Y); + + const float leftDeadZone = 10.0f; + + // Ось X левого джойстика + if (labs(leftStickX) > leftDeadZone) { + emit leftStickXMoved(leftStickX); + } else { + emit leftStickXMoved(0.0f); + } + + // Ось Y левого джойстика + if (labs(leftStickY) > leftDeadZone) { + emit leftStickYMoved(leftStickY); + } else { + emit leftStickYMoved(0.0f); + } + + // Сохраняем текущие значения левого джойстика + prevLeftStickX = leftStickX; + prevLeftStickY = leftStickY; + + // --- ОБРАБОТКА ПРАВОГО ДЖОЙСТИКА --- + float rightStickX = sf::Joystick::getAxisPosition(id, sf::Joystick::R); + float rightStickY = sf::Joystick::getAxisPosition(id, sf::Joystick::Z); + + const float rightDeadZone = 15.0f; + + // Ось X правого джойстика + if (labs(rightStickX) > rightDeadZone) { + emit rightStickXMoved(rightStickX); + } else { + emit rightStickXMoved(0.0f); + } + + // Ось Y правого джойстика + if (labs(rightStickY) > rightDeadZone) { + emit rightStickYMoved(rightStickY); + } else { + emit rightStickYMoved(0.0f); + } + + // Сохраняем текущие значения правого джойстика + prevRightStickX = rightStickX; + prevRightStickY = rightStickY; + + // --- ОБРАБОТКА ТРИГГЕРОВ L2/R2 (ОСИ U И V) --- + float leftTrigger = sf::Joystick::getAxisPosition(id, sf::Joystick::U); + float rightTrigger = sf::Joystick::getAxisPosition(id, sf::Joystick::V); + + // Триггеры обычно имеют диапазон от -100 до 100, + // но в покое могут быть -100, а при нажатии +100 (или наоборот) + // Приводим к диапазону 0-100 для удобства + float normalizedLeftTrigger = (leftTrigger + 100.0f) / 2.0f; // -100..100 -> 0..100 + float normalizedRightTrigger = (rightTrigger + 100.0f) / 2.0f; + + const float triggerThreshold = 30.0f; // Порог срабатывания (30%) + + // Левый триггер (L2) + if (normalizedLeftTrigger > triggerThreshold) { + if (labs(normalizedLeftTrigger - prevLeftTrigger) > 5.0f) { + emit leftTriggerMoved(normalizedLeftTrigger); + qDebug() << "Триггер L2:" << normalizedLeftTrigger << "%"; + } + } else { + if (prevLeftTrigger > triggerThreshold) { + emit leftTriggerMoved(0.0f); + } + } + + // Правый триггер (R2) + if (normalizedRightTrigger > triggerThreshold) { + if (labs(normalizedRightTrigger - prevRightTrigger) > 5.0f) { + emit rightTriggerMoved(normalizedRightTrigger); + qDebug() << "Триггер R2:" << normalizedRightTrigger << "%"; + } + } else { + if (prevRightTrigger > triggerThreshold) { + emit rightTriggerMoved(0.0f); + } + } + + prevLeftTrigger = normalizedLeftTrigger; + prevRightTrigger = normalizedRightTrigger; + + // --- ОБРАБОТКА КНОПОК L1/R1 (кнопки 4 и 5) --- + // На DEXP G-15 L1 и R1 обычно это кнопки 4 и 5 + bool currentL1State = sf::Joystick::isButtonPressed(id, 4); + bool currentR1State = sf::Joystick::isButtonPressed(id, 5); + + // Проверка состояния кнопок A/B/X/Y/Start/Back + bool currentButtonAState = sf::Joystick::isButtonPressed(id, BUTTON_A); + bool currentButtonBState = sf::Joystick::isButtonPressed(id, BUTTON_B); + bool currentButtonXState = sf::Joystick::isButtonPressed(id, BUTTON_X); + bool currentButtonYState = sf::Joystick::isButtonPressed(id, BUTTON_Y); + bool currentStartButtonState = sf::Joystick::isButtonPressed(id, START_BUTTON); + bool currentBackButtonState = sf::Joystick::isButtonPressed(id, BACK_BUTTON); + + // Обработка кнопки A + if (currentButtonAState && !prevButtonAState) { + emit buttonAPressed(); + qDebug() << "Кнопка A нажата на геймпаде"; + } + + // Обработка кнопки B + if (currentButtonBState && !prevButtonBState) { + emit buttonBPressed(); + qDebug() << "Кнопка B нажата на геймпаде"; + } + + // Обработка кнопки X + if (currentButtonXState && !prevButtonXState) { + emit buttonXPressed(); + qDebug() << "Кнопка X нажата на геймпаде"; + } + + // Обработка кнопки Y + if (currentButtonYState && !prevButtonYState) { + emit buttonYPressed(); + qDebug() << "Кнопка Y нажата на геймпаде"; + } + + // Обработка кнопки Start + if (currentStartButtonState && !prevStartButtonState) { + emit startButtonPressed(); + qDebug() << "Кнопка Start нажата на геймпаде"; + } + + // Обработка кнопки Back + if (currentBackButtonState && !prevBackButtonState) { + emit backButtonPressed(); + qDebug() << "Кнопка Back нажата на геймпаде"; + } + + // Обработка кнопки L1 + if (currentL1State && !prevL1State) { + emit L1Pressed(); + qDebug() << "Кнопка L1 нажата"; + } + + // Обработка кнопки R1 + if (currentR1State && !prevR1State) { + emit R1Pressed(); + qDebug() << "Кнопка R1 нажата"; + } + + // Сохраняем текущее состояние кнопок для следующего цикла + prevButtonAState = currentButtonAState; + prevButtonBState = currentButtonBState; + prevButtonXState = currentButtonXState; + prevButtonYState = currentButtonYState; + prevStartButtonState = currentStartButtonState; + prevBackButtonState = currentBackButtonState; + prevL1State = currentL1State; + prevR1State = currentR1State; + + // --- ОБРАБОТКА КРЕСТИКА (D-Pad) --- + float dPadX = sf::Joystick::getAxisPosition(id, sf::Joystick::PovX); + float dPadY = sf::Joystick::getAxisPosition(id, sf::Joystick::PovY); + + const float dPadThreshold = 80.0f; + + bool currentDPadUp = (dPadY <= -dPadThreshold); + bool currentDPadDown = (dPadY >= dPadThreshold); + bool currentDPadLeft = (dPadX <= -dPadThreshold); + bool currentDPadRight = (dPadX >= dPadThreshold); + + if (currentDPadUp && !prevDPadUp) { + emit dPadUpPressed(); + qDebug() << "Крестик: Вверх нажата"; + } + + if (currentDPadDown && !prevDPadDown) { + emit dPadDownPressed(); + qDebug() << "Крестик: Вниз нажата"; + } + + if (currentDPadLeft && !prevDPadLeft) { + emit dPadLeftPressed(); + qDebug() << "Крестик: Влево нажата"; + } + + if (currentDPadRight && !prevDPadRight) { + emit dPadRightPressed(); + qDebug() << "Крестик: Вправо нажата"; + } + + // Сохраняем текущее состояние крестика для следующего цикла + prevDPadUp = currentDPadUp; + prevDPadDown = currentDPadDown; + prevDPadLeft = currentDPadLeft; + prevDPadRight = currentDPadRight; + } +} + +QPair JoyStick::getLeftStickValues() const { + if (sf::Joystick::isConnected(id)) { + float y = sf::Joystick::getAxisPosition(id, sf::Joystick::Y); + float x = sf::Joystick::getAxisPosition(id, sf::Joystick::X); + return QPair(x, y); } + return QPair(0.0f, 0.0f); } +QPair JoyStick::getRightStickValues() const { + if (sf::Joystick::isConnected(id)) { + float x = sf::Joystick::getAxisPosition(id, sf::Joystick::R); + float y = sf::Joystick::getAxisPosition(id, sf::Joystick::Z); + return QPair(x, y); + } + return QPair(0.0f, 0.0f); +} + +// Метод для проверки подключения джойстика +bool JoyStick::isConnected() const { + return sf::Joystick::isConnected(id); +} + +// Метод для получения значений триггеров +QPair JoyStick::getTriggerValues() const { + if (sf::Joystick::isConnected(id)) { + float left = (sf::Joystick::getAxisPosition(id, sf::Joystick::U) + 100.0f) / 2.0f; + float right = (sf::Joystick::getAxisPosition(id, sf::Joystick::V) + 100.0f) / 2.0f; + return QPair(left, right); + } + return QPair(0.0f, 0.0f); +} \ No newline at end of file diff --git a/remote_control/joy_stick.h b/remote_control/joy_stick.h index 2f7371f..1d59170 100644 --- a/remote_control/joy_stick.h +++ b/remote_control/joy_stick.h @@ -7,8 +7,6 @@ #include "remote_control.h" #include "SFML/Window.hpp" - - class JoyStick : public RemoteControl { Q_OBJECT @@ -16,8 +14,87 @@ class JoyStick : public RemoteControl JoyStick(QObject *parent = nullptr); ~JoyStick(); +signals: + // Сигналы для осей джойстика + void leftStickXMoved(float value); // Левый стик: влево/вправо + void leftStickYMoved(float value); // Левый стик: вперед/назад + void rightStickXMoved(float value); // Правый стик: горизонталь + void rightStickYMoved(float value); // Правый стик: вертикаль + + // Сигналы для кнопок + void buttonAPressed(); // Кнопка A + void buttonBPressed(); // Кнопка B + void buttonXPressed(); // Кнопка X + void buttonYPressed(); // Кнопка Y + void startButtonPressed(); // Кнопка Start + void backButtonPressed(); // Кнопка Back + + + + void buttonXReleased(); + void buttonBReleased(); + + // Сигналы для триггеров + void leftTriggerMoved(float value); // L2 (аналоговый триггер) + void rightTriggerMoved(float value); // R2 (аналоговый триггер) + void L1Pressed(); // L1 (кнопка) + void R1Pressed(); // R1 (кнопка) + + // Сигналы для крестика (D-Pad) + void dPadUpPressed(); // Крестик: вверх + void dPadDownPressed(); // Крестик: вниз + void dPadLeftPressed(); // Крестик: влево + void dPadRightPressed(); // Крестик: вправо + private slots: void updateImpact(); + +public: + QPair getLeftStickValues() const; + QPair getRightStickValues() const; + QPair getTriggerValues() const; + bool isConnected() const; + +private: + // Идентификаторы кнопок для DEXP G-15 + const unsigned int BUTTON_A = 0; // Кнопка A + const unsigned int BUTTON_B = 1; // Кнопка B + const unsigned int BUTTON_X = 2; // Кнопка X + const unsigned int BUTTON_Y = 3; // Кнопка Y + const unsigned int START_BUTTON = 7; // Кнопка Start + const unsigned int BACK_BUTTON = 6; // Кнопка Back + + // Состояние предыдущих нажатий кнопок + bool prevButtonAState; + bool prevButtonBState; + bool prevButtonXState; + bool prevButtonYState; + bool prevStartButtonState; + bool prevBackButtonState; + + // Состояние кнопок L1/R1 + bool prevL1State; + bool prevR1State; + + // Состояние крестика (D-Pad) + bool prevDPadUp; + bool prevDPadDown; + bool prevDPadLeft; + bool prevDPadRight; + + // Предыдущие значения осей джойстиков + float prevLeftStickY; + float prevLeftStickX; + float prevRightStickY; + float prevRightStickX; + + // Предыдущие значения триггеров (оси) + float prevLeftTrigger; + float prevRightTrigger; + + // Оси для триггеров + sf::Joystick::Axis impactAxisLeftTrigger; + sf::Joystick::Axis impactAxisRightTrigger; }; -#endif // JOYSTICK_H +#endif // JOYSTICK_H \ No newline at end of file diff --git a/umas_settings.ini b/umas_settings.ini new file mode 100644 index 0000000..1fdda65 --- /dev/null +++ b/umas_settings.ini @@ -0,0 +1,26 @@ +[General] +CurrentSpeedMode=1 + +[SpeedMode_0] +depth=11 +pitch=12 +roll=13 +surge=14 +sway=15 +yaw=36 + +[SpeedMode_1] +depth=51 +pitch=52 +roll=53 +surge=57 +sway=55 +yaw=56 + +[SpeedMode_2] +depth=71 +pitch=72 +roll=73 +surge=74 +sway=60 +yaw=76