diff --git a/Servo/src/ServoController.cpp b/Servo/src/ServoController.cpp index 8a19c52..2075f8a 100644 --- a/Servo/src/ServoController.cpp +++ b/Servo/src/ServoController.cpp @@ -74,16 +74,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(); - // connect(timer, &QTimer::timeout, &this, &ServoController::handleGetRequestFromServo); - // timer->stop(); - } /*************************************************************************************************/ @@ -107,7 +100,7 @@ void ServoController::resetAzimuth() /*************************************************************************************************/ void ServoController::fix(double angle) { - changeMode(angle, angle, 5/6.0); + changeMode(angle, angle, 5/6.0); } /*************************************************************************************************/ @@ -162,7 +155,7 @@ double ServoController::getAngleOffset() /*************************************************************************************************/ 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)); } /*************************************************************************************************/ diff --git a/Servo/src/ServoController.cpp.autosave b/Servo/src/ServoController.cpp.autosave deleted file mode 100644 index 5b054a8..0000000 --- a/Servo/src/ServoController.cpp.autosave +++ /dev/null @@ -1,173 +0,0 @@ -#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; - -} - - -