From 11ab0aaca06e656dfdfbdb2cba462bba23f6e32e Mon Sep 17 00:00:00 2001 From: nasi Date: Wed, 17 Nov 2021 18:53:08 +0330 Subject: [PATCH] Add Get Status --- Servo/include/ModbusWrapper.h | 2 +- Servo/include/ServoController.h | 18 ++--- Servo/src/ModbusWrapper.cpp | 10 ++- Servo/src/ServoController.cpp | 116 ++++++++++++++++++-------------- Test/MainWindow.cpp | 13 ++-- Test/MainWindow.ui | 34 ++++++++++ 6 files changed, 127 insertions(+), 66 deletions(-) diff --git a/Servo/include/ModbusWrapper.h b/Servo/include/ModbusWrapper.h index d772090..94893ae 100644 --- a/Servo/include/ModbusWrapper.h +++ b/Servo/include/ModbusWrapper.h @@ -21,7 +21,7 @@ public: ~ModbusWrapper(); void connectToDevice(ModbusConfig modbusConfig); void init(); - + void close(); QBitArray getCoil(int startAddress, quint16 readSize); QBitArray getInputCoil(int startAddress, quint16 readSize); QVector getHoldingRegister(int startAddress, quint16 readSize); diff --git a/Servo/include/ServoController.h b/Servo/include/ServoController.h index 271655e..125470e 100644 --- a/Servo/include/ServoController.h +++ b/Servo/include/ServoController.h @@ -11,14 +11,14 @@ private: 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; - const int SRV_APPLY_SETTING_REG = 2004; - const int SRV_START_STOP_REG = 2005; - const int SRV_ZERO_OFFSET_ANGLE = 2007; - const int SRV_START_ANGLE_FRAC_PART_REG = 2010; - const int SRV_STOP_ANGLE_FRAC_PART_REG = 2011; - const int SRV_CALIBRATE_REG = 2013; - const int SRV_ENABLE_DRIVE_REG = 2017; - const int SRV_AZIMUTH_REG = 2030; + const int SRV_APPLY_SETTING_REG = 2003; //2004; + const int SRV_START_STOP_REG = 2004; //2005; + const int SRV_ZERO_OFFSET_ANGLE = 2005; //2007; + const int SRV_START_ANGLE_FRAC_PART_REG = 2006; //2010; + const int SRV_STOP_ANGLE_FRAC_PART_REG = 2007; //2011; + const int SRV_CALIBRATE_REG = 2008; //2013; + const int SRV_ENABLE_DRIVE_REG = 2009; //2017; + const int SRV_AZIMUTH_REG = 2010; //2030; const double SRV_ANGLE_FACTOR = 100.0; const double SRV_SPEED_FACTOR = 1000.0; @@ -48,6 +48,8 @@ public: double getAngleOffset(); void setAngleOffset(double offset); double getAzimuth(); + + bool getStatus(); }; #endif //SERVOCONTROLLER_H diff --git a/Servo/src/ModbusWrapper.cpp b/Servo/src/ModbusWrapper.cpp index 64ee357..b815323 100644 --- a/Servo/src/ModbusWrapper.cpp +++ b/Servo/src/ModbusWrapper.cpp @@ -80,7 +80,15 @@ void ModbusWrapper::init() if(exp.valid) { throw ServoException(exp.message); - } + } +} + +/*************************************************************************************************/ +void ModbusWrapper::close() +{ + _modbusMaster.close(); + _workerThread.quit(); + _workerThread.wait(); } /*************************************************************************************************/ diff --git a/Servo/src/ServoController.cpp b/Servo/src/ServoController.cpp index 9a65949..f3b0ef9 100644 --- a/Servo/src/ServoController.cpp +++ b/Servo/src/ServoController.cpp @@ -2,156 +2,170 @@ ModbusConfig ServoController::initiateConfig(QString serialPort) { - ModbusConfig _config; - _config.baud = QSerialPort::Baud19200; - _config.parity = QSerialPort::EvenParity; - _config.dataBits = QSerialPort::Data8; - _config.stopBits = QSerialPort::OneStop; - _config.responseTime = 2000; - _config.serialPort = serialPort; - _config.clientAddress = 1; + ModbusConfig _config; + _config.baud = QSerialPort::Baud19200; + _config.parity = QSerialPort::EvenParity; + _config.dataBits = QSerialPort::Data8; + _config.stopBits = QSerialPort::OneStop; + _config.responseTime = 2000; + _config.serialPort = serialPort; + _config.clientAddress = 1; - return _config; + return _config; } /*************************************************************************************************/ void ServoController::apply() { - _modbusWrapper.setSingleRegister(SRV_APPLY_SETTING_REG, SRV_APPLY_SETTING_VAL); + _modbusWrapper.setSingleRegister(SRV_APPLY_SETTING_REG, SRV_APPLY_SETTING_VAL); } /*************************************************************************************************/ void ServoController::enableDrive() { - _modbusWrapper.setSingleRegister(SRV_ENABLE_DRIVE_REG, 3); - apply(); + _modbusWrapper.setSingleRegister(SRV_ENABLE_DRIVE_REG, 3); + apply(); } /*************************************************************************************************/ void ServoController::setStartStop(bool start) { - _modbusWrapper.setSingleRegister(SRV_START_STOP_REG, start ? 3 : 0); - apply(); + _modbusWrapper.setSingleRegister(SRV_START_STOP_REG, start ? 3 : 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)); + _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)); + _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)); + _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(); + setStartAngle(startAngle); + setStopAngle(stopAngle); + setSpeed(speed); + setStartStop(true); + enableDrive(); + apply(); } /*************************************************************************************************/ void ServoController::init(QString serialPort) { - _modbusWrapper.init(); - _modbusWrapper.connectToDevice(initiateConfig(serialPort)); + _modbusWrapper.close(); + _modbusWrapper.init(); + _modbusWrapper.connectToDevice(initiateConfig(serialPort)); } /*************************************************************************************************/ void ServoController::startMoving() { - setStartStop(true); + setStartStop(true); } /*************************************************************************************************/ void ServoController::stopMoving() { - setStartStop(false); + setStartStop(false); } /*************************************************************************************************/ void ServoController::resetAzimuth() { - _modbusWrapper.setSingleRegister(SRV_CALIBRATE_REG, SRV_CALIBRATION_VAL); + _modbusWrapper.setSingleRegister(SRV_CALIBRATE_REG, SRV_CALIBRATION_VAL); } /*************************************************************************************************/ void ServoController::fix(double angle) { - changeMode(angle, angle, 5 / 6.0); + changeMode(angle, angle, 5 / 6.0); } /*************************************************************************************************/ void ServoController::sector(double startAngle, double stopAngle, double speed) { - changeMode(startAngle, stopAngle, speed); + changeMode(startAngle, stopAngle, speed); } /*************************************************************************************************/ double ServoController::getSpeed() { - auto speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG, 1); + auto speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG, 1); - return (speedVector[0] / SRV_SPEED_FACTOR); + return (speedVector[0] / SRV_SPEED_FACTOR); } /*************************************************************************************************/ double ServoController::getStopAngle() { - auto speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_PART_REG, 1); - auto speedVector2 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_FRAC_PART_REG, 1); + auto speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_PART_REG, 1); + auto speedVector2 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_FRAC_PART_REG, 1); - return (speedVector1[0] + speedVector2[0] / SRV_ANGLE_FACTOR); + return (speedVector1[0] + speedVector2[0] / SRV_ANGLE_FACTOR); } /*************************************************************************************************/ double ServoController::getStartAngle() { - auto speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_PART_REG, 1); - auto speedVector2 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_FRAC_PART_REG, 1); + auto speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_PART_REG, 1); + auto speedVector2 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_FRAC_PART_REG, 1); - return (speedVector1[0] + speedVector2[0] / SRV_ANGLE_FACTOR); + return (speedVector1[0] + speedVector2[0] / SRV_ANGLE_FACTOR); } /*************************************************************************************************/ double ServoController::getAngleOffset() { - auto angleOffsetVector = _modbusWrapper.getHoldingRegister(SRV_ZERO_OFFSET_ANGLE, 1); + auto angleOffsetVector = _modbusWrapper.getHoldingRegister(SRV_ZERO_OFFSET_ANGLE, 1); - return (angleOffsetVector[0] / SRV_ANGLE_FACTOR); + return (angleOffsetVector[0] / SRV_ANGLE_FACTOR); } /*************************************************************************************************/ void ServoController::setAngleOffset(double offset) { - _modbusWrapper.setSingleRegister(SRV_ZERO_OFFSET_ANGLE, - static_cast(offset * SRV_ANGLE_FACTOR)); + _modbusWrapper.setSingleRegister(SRV_ZERO_OFFSET_ANGLE, + static_cast(offset * SRV_ANGLE_FACTOR)); } /*************************************************************************************************/ double ServoController::getAzimuth() { - auto azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG, 1); + auto azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG, 1); - return (azimuthVector[0] / SRV_ANGLE_FACTOR); + return (azimuthVector[0] / SRV_ANGLE_FACTOR); +} + +/*************************************************************************************************/ +bool ServoController::getStatus() +{ + try { + getSpeed(); + return true; + + } catch (ServoException ex) { + _modbusWrapper.close(); + return false; + } } diff --git a/Test/MainWindow.cpp b/Test/MainWindow.cpp index 0ef906c..f39d343 100644 --- a/Test/MainWindow.cpp +++ b/Test/MainWindow.cpp @@ -212,15 +212,18 @@ void MainWindow::on_init_clicked() void MainWindow::handleGetRequestFromServo() { try { - ui->showSpeed->setText(QString::number(_servoControler.getSpeed())); - ui->showStartAngle->setText(QString::number(_servoControler.getStartAngle())); - ui->showStopAngle->setText(QString::number(_servoControler.getStopAngle())); - ui->showAngleOffset->setText(QString::number(_servoControler.getAngleOffset())); - ui->showAzimuth->setText(QString::number(_servoControler.getAzimuth())); + // ui->showSpeed->setText(QString::number(_servoControler.getSpeed())); + //ui->showStartAngle->setText(QString::number(_servoControler.getStartAngle())); + //ui->showStopAngle->setText(QString::number(_servoControler.getStopAngle())); + //ui->showAngleOffset->setText(QString::number(_servoControler.getAngleOffset())); + //ui->showAzimuth->setText(QString::number(_servoControler.getAzimuth())); + ui->connectionState->setText(QVariant(_servoControler.getStatus()).toString()); } catch(ServoException exp) { + timer->stop(); ui->showServoError->setText(exp.getMessage()); + } } diff --git a/Test/MainWindow.ui b/Test/MainWindow.ui index 706f62d..77c77ca 100644 --- a/Test/MainWindow.ui +++ b/Test/MainWindow.ui @@ -621,6 +621,40 @@ background-color: rgb(186, 178, 158); + + + + 180 + 0 + 181 + 17 + + + + Connection State to Client: + + + + + + 370 + 0 + 121 + 20 + + + + background-color: rgb(52, 101, 164); +color: rgb(238, 238, 236); + + + + + + + 0 + +