diff --git a/Servo/include/ServoController.h b/Servo/include/ServoController.h
index 7a37782..bcf71d5 100644
--- a/Servo/include/ServoController.h
+++ b/Servo/include/ServoController.h
@@ -1,11 +1,61 @@
 #ifndef SERVOCONTROLLER_H
 #define SERVOCONTROLLER_H
+#include <QTimer>
 
-
+#include "ModbusWrapper.h"
+#include "ModbusConfig.h"
 class ServoController
 {
 public:
-	ServoController();
+    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;
+    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_ANGLE_FACTOR = 100;
+    const int SRV_SPEED_FACTOR = 1000;
+    const quint16 SRV_APPLY_SETTING_VAL = 1000;
+    const quint16 SRV_CALIBRATION_VAL = 13576;
+
+
+private:
+    void apply();
+    void enableDrive();
+    void setStartStop(bool start);
+    void setStartAngle(double angle);
+    void setStopAngle(double angle);
+    void setSpeed(double speed);
+    void changeMode(double startAngle, double stopAngle, double speed);
+    ModbusConfig initiateConfig();
+
+
+public:
+    void init();
+    void startMoving();
+    void stopMoving();
+    void resetAzimuth();
+    void fix(double angle);
+    void sector(double startAngle, double stopAngle, double speed);
+
+    double getSpeed();
+    double getStopAngle();
+    double getStartAngle();
+    double getAngleOffset();
+    void setAngleOffset(double offset);
+    double getAzimuth();
+
+
 };
 
 #endif // SERVOCONTROLLER_H
diff --git a/Servo/src/ServoController.cpp b/Servo/src/ServoController.cpp
index 449faa7..70b34c4 100644
--- a/Servo/src/ServoController.cpp
+++ b/Servo/src/ServoController.cpp
@@ -1,5 +1,178 @@
 #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.dataBits = QSerialPort::Data8;
+    _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<quint16>(angle));
+    _modbusWrapper.setSingleRegister(SRV_START_ANGLE_FRAC_PART_REG, static_cast<quint16>((angle-(static_cast<int>(angle)))*100));
+}
+
+/*************************************************************************************************/
+void ServoController::setStopAngle(double angle)
+{
+    _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_INT_PART_REG, static_cast<quint16>(angle));
+    _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_FRAC_PART_REG, static_cast<quint16>((angle-(static_cast<int>(angle)))*100));
+}
+
+/*************************************************************************************************/
+void ServoController::setSpeed(double speed)
+{
+    _modbusWrapper.setSingleRegister(SRV_AZIMUTH_SPEED_REG, static_cast<quint16>(speed*SRV_SPEED_FACTOR));
+}
+
+/*************************************************************************************************/
+void ServoController::changeMode(double startAngle, double stopAngle, double speed)
+{
+    setStartAngle(startAngle);
+    setStopAngle(stopAngle);
+    setStartStop(true);
+    enableDrive();
+    apply();
+}
+
+/*************************************************************************************************/
+void ServoController::init()
+{
+
+    _modbusWrapper.connectToDevice(initiateConfig());
+    // timer = new QTimer();
+    //  timer->setInterval(2000);
+    // timer->start();
+    // connect(timer, &QTimer::timeout, &this, &ServoController::handleGetRequestFromServo);
+    // timer->stop();
+
+}
+
+/*************************************************************************************************/
+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);
+}
+
+/*************************************************************************************************/
+void ServoController::sector(double startAngle, double stopAngle, double speed)
+{
+    changeMode(startAngle, stopAngle, speed);
+}
+
+/*************************************************************************************************/
+double ServoController::getSpeed()
+{
+    double _speed;
+    QVector<quint16> _speedVector;
+    _speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG,1 );
+    _speed  = _speedVector[0] / SRV_SPEED_FACTOR;
+    return _speed;
+}
+
+/*************************************************************************************************/
+double ServoController::getStopAngle()
+{
+    double _speed;
+    QVector<quint16> _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<quint16> _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<quint16> _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<quint16>(offset*SRV_ANGLE_FACTOR));
+}
+
+/*************************************************************************************************/
+double ServoController::getAzimuth()
+{
+    double _azimuth;
+    QVector<quint16> _azimuthVector;
+    _azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG,1 );
+    _azimuth  = _azimuthVector[0] / SRV_ANGLE_FACTOR;
+    return _azimuth;
+
+}
+
+
+