h-4nd-h
/
Servo
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Add code to servoControler class

test
nasi 3 years ago
parent
731bd1ffea
commit
6b11392945
7 changed files with 446 additions and 33 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      Servo/include/ServoController.h
  2. 64
      Servo/src/ServoController.cpp
  3. 173
      Servo/src/ServoController.cpp.autosave
  4. 16
      Test/MainWindow.cpp
  5. 3
      Test/MainWindow.h
  6. 44
      Test/MainWindow.h.autosave
  7. 178
      Test/MainWindow.ui

1
Servo/include/ServoController.h
View File

@ -10,7 +10,6 @@ public:
ServoController(); ServoController();
private: private:
ModbusWrapper _modbusWrapper; ModbusWrapper _modbusWrapper;
QTimer *timer;
const int SRV_START_ANGLE_INT_PART_REG = 2000; const int SRV_START_ANGLE_INT_PART_REG = 2000;
const int SRV_STOP_ANGLE_INT_PART_REG = 2001; const int SRV_STOP_ANGLE_INT_PART_REG = 2001;
const int SRV_AZIMUTH_SPEED_REG = 2002; const int SRV_AZIMUTH_SPEED_REG = 2002;

64
Servo/src/ServoController.cpp
View File

@ -13,7 +13,6 @@ ModbusConfig ServoController::initiateConfig()
_config.parity = QSerialPort::EvenParity; _config.parity = QSerialPort::EvenParity;
_config.dataBits = QSerialPort::Data8; _config.dataBits = QSerialPort::Data8;
_config.stopBits = QSerialPort::OneStop; _config.stopBits = QSerialPort::OneStop;
_config.dataBits = QSerialPort::Data8;
_config.responseTime = 2000; _config.responseTime = 2000;
return _config; return _config;
@ -45,14 +44,14 @@ void ServoController::setStartStop(bool start)
void ServoController::setStartAngle(double angle) void ServoController::setStartAngle(double angle)
{ {
_modbusWrapper.setSingleRegister(SRV_START_ANGLE_INT_PART_REG, static_cast<quint16>(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)); _modbusWrapper.setSingleRegister(SRV_START_ANGLE_FRAC_PART_REG, static_cast<quint16>((angle - (static_cast<int>(angle))) * 100));
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::setStopAngle(double angle) void ServoController::setStopAngle(double angle)
{ {
_modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_INT_PART_REG, static_cast<quint16>(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)); _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_FRAC_PART_REG, static_cast<quint16>((angle - (static_cast<int>(angle))) * 100));
} }
/*************************************************************************************************/ /*************************************************************************************************/
@ -66,6 +65,7 @@ void ServoController::changeMode(double startAngle, double stopAngle, double spe
{ {
setStartAngle(startAngle); setStartAngle(startAngle);
setStopAngle(stopAngle); setStopAngle(stopAngle);
setSpeed(speed);
setStartStop(true); setStartStop(true);
enableDrive(); enableDrive();
apply(); apply();
@ -75,7 +75,9 @@ void ServoController::changeMode(double startAngle, double stopAngle, double spe
void ServoController::init() void ServoController::init()
{ {
_modbusWrapper.init();
_modbusWrapper.connectToDevice(initiateConfig()); _modbusWrapper.connectToDevice(initiateConfig());
// timer = new QTimer(); // timer = new QTimer();
// timer->setInterval(2000); // timer->setInterval(2000);
// timer->start(); // timer->start();
@ -105,7 +107,7 @@ void ServoController::resetAzimuth()
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::fix(double angle) 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 ServoController::getSpeed()
{ {
double _speed; double speed;
QVector<quint16> _speedVector; QVector<quint16> speedVector;
_speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG,1 ); speedVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_SPEED_REG,1);
_speed = _speedVector[0] / SRV_SPEED_FACTOR; speed = speedVector[0] / SRV_SPEED_FACTOR;
return _speed; return speed;
} }
/*************************************************************************************************/ /*************************************************************************************************/
double ServoController::getStopAngle() double ServoController::getStopAngle()
{ {
double _speed; double speed;
QVector<quint16> _speedVector1, _speedVector2; QVector<quint16> speedVector1, speedVector2;
_speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_PART_REG,1 ); speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_PART_REG,1);
_speedVector2 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_FRAC_PART_REG,1 ); speedVector2 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_FRAC_PART_REG,1);
_speed = (_speedVector1[0]+_speedVector2[0]) / SRV_ANGLE_FACTOR; speed = (speedVector1[0] + speedVector2[0]) / SRV_ANGLE_FACTOR;
return _speed; return speed;
} }
/*************************************************************************************************/ /*************************************************************************************************/
double ServoController::getStartAngle() double ServoController::getStartAngle()
{ {
double _speed; double speed;
QVector<quint16> _speedVector1, _speedVector2; QVector<quint16> speedVector1, speedVector2;
_speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_PART_REG,1 ); speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_PART_REG,1);
_speedVector2 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_FRAC_PART_REG,1 ); speedVector2 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_FRAC_PART_REG,1);
_speed = (_speedVector1[0]+_speedVector2[0]) / SRV_ANGLE_FACTOR; speed = (speedVector1[0] + speedVector2[0]) / SRV_ANGLE_FACTOR;
return _speed; return speed;
} }
/*************************************************************************************************/ /*************************************************************************************************/
double ServoController::getAngleOffset() double ServoController::getAngleOffset()
{ {
double _angleOffset; double angleOffset;
QVector<quint16> _angleOffsetVector; QVector<quint16> angleOffsetVector;
_angleOffsetVector = _modbusWrapper.getHoldingRegister(SRV_ZERO_OFFSET_ANGLE,1 ); angleOffsetVector = _modbusWrapper.getHoldingRegister(SRV_ZERO_OFFSET_ANGLE,1);
_angleOffset = _angleOffsetVector[0] / SRV_ANGLE_FACTOR; angleOffset = angleOffsetVector[0] / SRV_ANGLE_FACTOR;
return _angleOffset; return angleOffset;
} }
@ -166,11 +168,11 @@ void ServoController::setAngleOffset(double offset)
/*************************************************************************************************/ /*************************************************************************************************/
double ServoController::getAzimuth() double ServoController::getAzimuth()
{ {
double _azimuth; double azimuth;
QVector<quint16> _azimuthVector; QVector<quint16> azimuthVector;
_azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG,1 ); azimuthVector = _modbusWrapper.getHoldingRegister(SRV_AZIMUTH_REG,1);
_azimuth = _azimuthVector[0] / SRV_ANGLE_FACTOR; azimuth = azimuthVector[0] / SRV_ANGLE_FACTOR;
return _azimuth; return azimuth;
} }

173
Servo/src/ServoController.cpp.autosave
View File

@ -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<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);
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<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;
}

16
Test/MainWindow.cpp
View File

@ -191,3 +191,19 @@ void MainWindow::on_writeMultiRegister_clicked()
ui->errorMonitoring->setText(exp.getMessage()); 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();
}

3
Test/MainWindow.h
View File

@ -5,6 +5,7 @@
#include "ModbusWrapper.h" #include "ModbusWrapper.h"
#include "ModbusConfig.h" #include "ModbusConfig.h"
#include "ServoController.h"
QT_BEGIN_NAMESPACE QT_BEGIN_NAMESPACE
namespace Ui { class MainWindow; namespace Ui { class MainWindow;
@ -19,6 +20,7 @@ private:
Ui::MainWindow* ui; Ui::MainWindow* ui;
ModbusWrapper modbusWrapper; ModbusWrapper modbusWrapper;
ModbusConfig configDevice; ModbusConfig configDevice;
ServoController _servoControler;
public: public:
MainWindow(QWidget* parent = nullptr); MainWindow(QWidget* parent = nullptr);
@ -35,6 +37,7 @@ private slots:
void on_writeMultiCoil_clicked(); void on_writeMultiCoil_clicked();
void on_writeSingleRegister_clicked(); void on_writeSingleRegister_clicked();
void on_writeMultiRegister_clicked(); void on_writeMultiRegister_clicked();
void on_fix_clicked();
}; };
#endif //MAINWINDOW_H #endif //MAINWINDOW_H

44
Test/MainWindow.h.autosave
View File

@ -0,0 +1,44 @@
#ifndef MAINWINDOW_H
#define MAINWINDOW_H
#include <QMainWindow>
#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<quint16> 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

178
Test/MainWindow.ui
View File

@ -14,7 +14,7 @@
<string>MainWindow</string> <string>MainWindow</string>
</property> </property>
<widget class="QWidget" name="centralwidget"> <widget class="QWidget" name="centralwidget">
<widget class="QTabWidget" name="tabWidget"> <widget class="QTabWidget" name="init">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>10</x> <x>10</x>
@ -23,6 +23,9 @@
<height>621</height> <height>621</height>
</rect> </rect>
</property> </property>
<property name="currentIndex">
<number>1</number>
</property>
<widget class="QWidget" name="tab_5"> <widget class="QWidget" name="tab_5">
<attribute name="title"> <attribute name="title">
<string>Tab 1</string> <string>Tab 1</string>
@ -527,6 +530,179 @@
<attribute name="title"> <attribute name="title">
<string>Tab 2</string> <string>Tab 2</string>
</attribute> </attribute>
<widget class="QGroupBox" name="groupBox_11">
<property name="geometry">
<rect>
<x>30</x>
<y>20</y>
<width>471</width>
<height>71</height>
</rect>
</property>
<property name="title">
<string>Run Order</string>
</property>
<widget class="QPushButton" name="ResetAzimuth">
<property name="geometry">
<rect>
<x>340</x>
<y>30</y>
<width>111</width>
<height>25</height>
</rect>
</property>
<property name="text">
<string>ResetAzimuth</string>
</property>
</widget>
<widget class="QPushButton" name="pushButton">
<property name="geometry">
<rect>
<x>28</x>
<y>30</y>
<width>61</width>
<height>25</height>
</rect>
</property>
<property name="text">
<string>Init</string>
</property>
</widget>
<widget class="QPushButton" name="startMoving">
<property name="geometry">
<rect>
<x>110</x>
<y>30</y>
<width>91</width>
<height>25</height>
</rect>
</property>
<property name="text">
<string>StartMoving</string>
</property>
</widget>
<widget class="QPushButton" name="stopMoving">
<property name="geometry">
<rect>
<x>210</x>
<y>30</y>
<width>91</width>
<height>25</height>
</rect>
</property>
<property name="text">
<string>StopMoving</string>
</property>
</widget>
</widget>
<widget class="QGroupBox" name="groupBox_12">
<property name="geometry">
<rect>
<x>30</x>
<y>110</y>
<width>471</width>
<height>161</height>
</rect>
</property>
<property name="title">
<string>GroupBox</string>
</property>
<widget class="QLineEdit" name="startAngle">
<property name="geometry">
<rect>
<x>100</x>
<y>30</y>
<width>81</width>
<height>25</height>
</rect>
</property>
</widget>
<widget class="QLabel" name="label_3">
<property name="geometry">
<rect>
<x>20</x>
<y>30</y>
<width>81</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>StartAngle</string>
</property>
</widget>
<widget class="QLabel" name="label_24">
<property name="geometry">
<rect>
<x>20</x>
<y>70</y>
<width>81</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>StopAngle</string>
</property>
</widget>
<widget class="QLineEdit" name="stopAngle">
<property name="geometry">
<rect>
<x>100</x>
<y>70</y>
<width>81</width>
<height>25</height>
</rect>
</property>
</widget>
<widget class="QLabel" name="label_25">
<property name="geometry">
<rect>
<x>20</x>
<y>110</y>
<width>71</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>Speed</string>
</property>
</widget>
<widget class="QLineEdit" name="speed">
<property name="geometry">
<rect>
<x>80</x>
<y>110</y>
<width>101</width>
<height>25</height>
</rect>
</property>
</widget>
<widget class="QPushButton" name="fix">
<property name="geometry">
<rect>
<x>240</x>
<y>60</y>
<width>89</width>
<height>25</height>
</rect>
</property>
<property name="text">
<string>Fix Speed</string>
</property>
</widget>
<widget class="QPushButton" name="sectorSpeed">
<property name="geometry">
<rect>
<x>230</x>
<y>100</y>
<width>111</width>
<height>25</height>
</rect>
</property>
<property name="text">
<string>Sector Speed</string>
</property>
</widget>
</widget>
</widget> </widget>
</widget> </widget>
</widget> </widget>

Write Preview
Loading…
Cancel
Save