From 6b113929453a4beb22260f3d180bd36e10edcdff Mon Sep 17 00:00:00 2001 From: nasi Date: Mon, 8 Nov 2021 20:13:21 +0330 Subject: [PATCH] Add code to servoControler class --- Servo/include/ServoController.h | 1 - Servo/src/ServoController.cpp | 64 ++++----- Servo/src/ServoController.cpp.autosave | 173 ++++++++++++++++++++++++ Test/MainWindow.cpp | 16 +++ Test/MainWindow.h | 3 + Test/MainWindow.h.autosave | 44 ++++++ Test/MainWindow.ui | 178 ++++++++++++++++++++++++- 7 files changed, 446 insertions(+), 33 deletions(-) create mode 100644 Servo/src/ServoController.cpp.autosave create mode 100644 Test/MainWindow.h.autosave diff --git a/Servo/include/ServoController.h b/Servo/include/ServoController.h index bcf71d5..abdbe90 100644 --- a/Servo/include/ServoController.h +++ b/Servo/include/ServoController.h @@ -10,7 +10,6 @@ public: ServoController(); private: ModbusWrapper _modbusWrapper; - QTimer *timer; const int SRV_START_ANGLE_INT_PART_REG = 2000; const int SRV_STOP_ANGLE_INT_PART_REG = 2001; const int SRV_AZIMUTH_SPEED_REG = 2002; diff --git a/Servo/src/ServoController.cpp b/Servo/src/ServoController.cpp index 70b34c4..8a19c52 100644 --- a/Servo/src/ServoController.cpp +++ b/Servo/src/ServoController.cpp @@ -13,7 +13,6 @@ ModbusConfig ServoController::initiateConfig() _config.parity = QSerialPort::EvenParity; _config.dataBits = QSerialPort::Data8; _config.stopBits = QSerialPort::OneStop; - _config.dataBits = QSerialPort::Data8; _config.responseTime = 2000; return _config; @@ -45,14 +44,14 @@ void ServoController::setStartStop(bool start) void ServoController::setStartAngle(double angle) { _modbusWrapper.setSingleRegister(SRV_START_ANGLE_INT_PART_REG, static_cast(angle)); - _modbusWrapper.setSingleRegister(SRV_START_ANGLE_FRAC_PART_REG, static_cast((angle-(static_cast(angle)))*100)); + _modbusWrapper.setSingleRegister(SRV_START_ANGLE_FRAC_PART_REG, static_cast((angle - (static_cast(angle))) * 100)); } /*************************************************************************************************/ void ServoController::setStopAngle(double angle) { _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_INT_PART_REG, static_cast(angle)); - _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_FRAC_PART_REG, static_cast((angle-(static_cast(angle)))*100)); + _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_FRAC_PART_REG, static_cast((angle - (static_cast(angle))) * 100)); } /*************************************************************************************************/ @@ -66,6 +65,7 @@ void ServoController::changeMode(double startAngle, double stopAngle, double spe { setStartAngle(startAngle); setStopAngle(stopAngle); + setSpeed(speed); setStartStop(true); enableDrive(); apply(); @@ -75,7 +75,9 @@ void ServoController::changeMode(double startAngle, double stopAngle, double spe void ServoController::init() { + _modbusWrapper.init(); _modbusWrapper.connectToDevice(initiateConfig()); + // timer = new QTimer(); // timer->setInterval(2000); // timer->start(); @@ -105,7 +107,7 @@ void ServoController::resetAzimuth() /*************************************************************************************************/ void ServoController::fix(double angle) { - changeMode(angle, angle, 5/6); + changeMode(angle, angle, 5/6.0); } /*************************************************************************************************/ @@ -117,43 +119,43 @@ void ServoController::sector(double startAngle, double stopAngle, double speed) /*************************************************************************************************/ double ServoController::getSpeed() { - double _speed; - QVector _speedVector; - _speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG,1 ); - _speed = _speedVector[0] / SRV_SPEED_FACTOR; - return _speed; + double speed; + QVector speedVector; + speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG,1); + speed = speedVector[0] / SRV_SPEED_FACTOR; + return speed; } /*************************************************************************************************/ double ServoController::getStopAngle() { - double _speed; - QVector _speedVector1, _speedVector2; - _speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_PART_REG,1 ); - _speedVector2 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_FRAC_PART_REG,1 ); - _speed = (_speedVector1[0]+_speedVector2[0]) / SRV_ANGLE_FACTOR; - return _speed; + double speed; + QVector speedVector1, speedVector2; + speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_PART_REG,1); + speedVector2 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_FRAC_PART_REG,1); + speed = (speedVector1[0] + speedVector2[0]) / SRV_ANGLE_FACTOR; + return speed; } /*************************************************************************************************/ double ServoController::getStartAngle() { - double _speed; - QVector _speedVector1, _speedVector2; - _speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_PART_REG,1 ); - _speedVector2 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_FRAC_PART_REG,1 ); - _speed = (_speedVector1[0]+_speedVector2[0]) / SRV_ANGLE_FACTOR; - return _speed; + double speed; + QVector speedVector1, speedVector2; + speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_PART_REG,1); + speedVector2 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_FRAC_PART_REG,1); + speed = (speedVector1[0] + speedVector2[0]) / SRV_ANGLE_FACTOR; + return speed; } /*************************************************************************************************/ double ServoController::getAngleOffset() { - double _angleOffset; - QVector _angleOffsetVector; - _angleOffsetVector = _modbusWrapper.getHoldingRegister(SRV_ZERO_OFFSET_ANGLE,1 ); - _angleOffset = _angleOffsetVector[0] / SRV_ANGLE_FACTOR; - return _angleOffset; + double angleOffset; + QVector angleOffsetVector; + angleOffsetVector = _modbusWrapper.getHoldingRegister(SRV_ZERO_OFFSET_ANGLE,1); + angleOffset = angleOffsetVector[0] / SRV_ANGLE_FACTOR; + return angleOffset; } @@ -166,11 +168,11 @@ void ServoController::setAngleOffset(double offset) /*************************************************************************************************/ double ServoController::getAzimuth() { - double _azimuth; - QVector _azimuthVector; - _azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG,1 ); - _azimuth = _azimuthVector[0] / SRV_ANGLE_FACTOR; - return _azimuth; + double azimuth; + QVector azimuthVector; + azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG,1); + azimuth = azimuthVector[0] / SRV_ANGLE_FACTOR; + return azimuth; } diff --git a/Servo/src/ServoController.cpp.autosave b/Servo/src/ServoController.cpp.autosave new file mode 100644 index 0000000..5b054a8 --- /dev/null +++ b/Servo/src/ServoController.cpp.autosave @@ -0,0 +1,173 @@ +#include "ServoController.h" + + +ServoController::ServoController() +{ +} + +/*************************************************************************************************/ +ModbusConfig ServoController::initiateConfig() +{ + ModbusConfig _config; + _config.baud =QSerialPort::Baud19200; + _config.parity = QSerialPort::EvenParity; + _config.dataBits = QSerialPort::Data8; + _config.stopBits = QSerialPort::OneStop; + _config.responseTime = 2000; + return _config; + +} + +/*************************************************************************************************/ +void ServoController::apply() +{ + _modbusWrapper.setSingleRegister(SRV_APPLY_SETTING_REG, SRV_APPLY_SETTING_VAL); +} + +/*************************************************************************************************/ +void ServoController::enableDrive() +{ + _modbusWrapper.setSingleRegister(SRV_ENABLE_DRIVE_REG, 3); + apply(); +} + +void ServoController::setStartStop(bool start) +{ + if(start) + _modbusWrapper.setSingleRegister(SRV_START_STOP_REG, 3); + else + _modbusWrapper.setSingleRegister(SRV_START_STOP_REG, 0); + apply(); +} + +/*************************************************************************************************/ +void ServoController::setStartAngle(double angle) +{ + _modbusWrapper.setSingleRegister(SRV_START_ANGLE_INT_PART_REG, static_cast(angle)); + _modbusWrapper.setSingleRegister(SRV_START_ANGLE_FRAC_PART_REG, static_cast((angle - (static_cast(angle))) * 100)); +} + +/*************************************************************************************************/ +void ServoController::setStopAngle(double angle) +{ + _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_INT_PART_REG, static_cast(angle)); + _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_FRAC_PART_REG, static_cast((angle - (static_cast(angle))) * 100)); +} + +/*************************************************************************************************/ +void ServoController::setSpeed(double speed) +{ + _modbusWrapper.setSingleRegister(SRV_AZIMUTH_SPEED_REG, static_cast(speed*SRV_SPEED_FACTOR)); +} + +/*************************************************************************************************/ +void ServoController::changeMode(double startAngle, double stopAngle, double speed) +{ + setStartAngle(startAngle); + setStopAngle(stopAngle); + setSpeed(speed); + setStartStop(true); + enableDrive(); + apply(); +} + +/*************************************************************************************************/ +void ServoController::init() +{ + _modbusWrapper.init(); + _modbusWrapper.connectToDevice(initiateConfig()); + +} + +/*************************************************************************************************/ +void ServoController::startMoving() +{ + setStartStop(true); +} + +/*************************************************************************************************/ +void ServoController::stopMoving() +{ + setStartStop(false); +} + +/*************************************************************************************************/ +void ServoController::resetAzimuth() +{ + _modbusWrapper.setSingleRegister(SRV_CALIBRATE_REG, SRV_CALIBRATION_VAL); +} + +/*************************************************************************************************/ +void ServoController::fix(double angle) +{ + changeMode(angle, angle, 5/6.0); +} + +/*************************************************************************************************/ +void ServoController::sector(double startAngle, double stopAngle, double speed) +{ + changeMode(startAngle, stopAngle, speed); +} + +/*************************************************************************************************/ +double ServoController::getSpeed() +{ + double speed; + QVector speedVector; + speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG,1); + speed = speedVector[0] / SRV_SPEED_FACTOR; + return speed; +} + +/*************************************************************************************************/ +double ServoController::getStopAngle() +{ + double speed; + QVector speedVector1, speedVector2; + speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_PART_REG,1); + speedVector2 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_FRAC_PART_REG,1); + speed = (speedVector1[0] + speedVector2[0]) / SRV_ANGLE_FACTOR; + return speed; +} + +/*************************************************************************************************/ +double ServoController::getStartAngle() +{ + double speed; + QVector speedVector1, speedVector2; + speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_PART_REG,1); + speedVector2 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_FRAC_PART_REG,1); + speed = (speedVector1[0] + speedVector2[0]) / SRV_ANGLE_FACTOR; + return speed; +} + +/*************************************************************************************************/ +double ServoController::getAngleOffset() +{ + double angleOffset; + QVector angleOffsetVector; + angleOffsetVector = _modbusWrapper.getHoldingRegister(SRV_ZERO_OFFSET_ANGLE,1); + angleOffset = angleOffsetVector[0] / SRV_ANGLE_FACTOR; + return angleOffset; + +} + +/*************************************************************************************************/ +void ServoController::setAngleOffset(double offset) +{ + _modbusWrapper.setSingleRegister(SRV_ZERO_OFFSET_ANGLE, static_cast(offset*SRV_ANGLE_FACTOR)); +} + +/*************************************************************************************************/ +double ServoController::getAzimuth() +{ + double azimuth; + QVector azimuthVector; + azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG,1); + azimuth = azimuthVector[0] / SRV_ANGLE_FACTOR; + return azimuth; + +} + + + diff --git a/Test/MainWindow.cpp b/Test/MainWindow.cpp index 627b4bc..b4036e7 100644 --- a/Test/MainWindow.cpp +++ b/Test/MainWindow.cpp @@ -191,3 +191,19 @@ void MainWindow::on_writeMultiRegister_clicked() ui->errorMonitoring->setText(exp.getMessage()); } } + +void MainWindow::on_fix_clicked() +{ + _servoControler.fix(11); +} + +void MainWindow::on_pushButton_clicked() +{ + + // timer = new QTimer(); + // timer->setInterval(2000); + // timer->start(); + // connect(timer, &QTimer::timeout, &this, &ServoController::handleGetRequestFromServo); + // timer->stop(); + +} diff --git a/Test/MainWindow.h b/Test/MainWindow.h index 6b63513..e959567 100644 --- a/Test/MainWindow.h +++ b/Test/MainWindow.h @@ -5,6 +5,7 @@ #include "ModbusWrapper.h" #include "ModbusConfig.h" +#include "ServoController.h" QT_BEGIN_NAMESPACE namespace Ui { class MainWindow; @@ -19,6 +20,7 @@ private: Ui::MainWindow* ui; ModbusWrapper modbusWrapper; ModbusConfig configDevice; + ServoController _servoControler; public: MainWindow(QWidget* parent = nullptr); @@ -35,6 +37,7 @@ private slots: void on_writeMultiCoil_clicked(); void on_writeSingleRegister_clicked(); void on_writeMultiRegister_clicked(); + void on_fix_clicked(); }; #endif //MAINWINDOW_H diff --git a/Test/MainWindow.h.autosave b/Test/MainWindow.h.autosave new file mode 100644 index 0000000..af50f2c --- /dev/null +++ b/Test/MainWindow.h.autosave @@ -0,0 +1,44 @@ +#ifndef MAINWINDOW_H +#define MAINWINDOW_H + +#include + +#include "ModbusWrapper.h" +#include "ModbusConfig.h" +#include "ServoController.h" + +QT_BEGIN_NAMESPACE +namespace Ui { class MainWindow; +} +QT_END_NAMESPACE + +class MainWindow : public QMainWindow +{ + Q_OBJECT + +private: + Ui::MainWindow* ui; + ModbusWrapper modbusWrapper; + ModbusConfig configDevice; + ServoController _servoControler; + +public: + MainWindow(QWidget* parent = nullptr); + ~MainWindow(); + void printCoilsDataFromClient(QBitArray uiCoils); + void printRegisterDataFromClient(QVector uiHoldingRegisters); + + //uncrustify off +private slots: + void on_connect_clicked(); + //uncrustify on + void on_readButton_clicked(); + void on_writeSingleCoil_clicked(); + void on_writeMultiCoil_clicked(); + void on_writeSingleRegister_clicked(); + void on_writeMultiRegister_clicked(); + void on_fix_clicked(); + void on_pushButton_clicked(); +}; + +#endif //MAINWINDOW_H diff --git a/Test/MainWindow.ui b/Test/MainWindow.ui index b7704f4..c00666e 100644 --- a/Test/MainWindow.ui +++ b/Test/MainWindow.ui @@ -14,7 +14,7 @@ MainWindow - + 10 @@ -23,6 +23,9 @@ 621 + + 1 + Tab 1 @@ -527,6 +530,179 @@ Tab 2 + + + + 30 + 20 + 471 + 71 + + + + Run Order + + + + + 340 + 30 + 111 + 25 + + + + ResetAzimuth + + + + + + 28 + 30 + 61 + 25 + + + + Init + + + + + + 110 + 30 + 91 + 25 + + + + StartMoving + + + + + + 210 + 30 + 91 + 25 + + + + StopMoving + + + + + + + 30 + 110 + 471 + 161 + + + + GroupBox + + + + + 100 + 30 + 81 + 25 + + + + + + + 20 + 30 + 81 + 20 + + + + StartAngle + + + + + + 20 + 70 + 81 + 20 + + + + StopAngle + + + + + + 100 + 70 + 81 + 25 + + + + + + + 20 + 110 + 71 + 20 + + + + Speed + + + + + + 80 + 110 + 101 + 25 + + + + + + + 240 + 60 + 89 + 25 + + + + Fix Speed + + + + + + 230 + 100 + 111 + 25 + + + + Sector Speed + + +