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

Add Get Status

AddStatus
nasi 3 years ago
parent
55ddea8ec1
commit
11ab0aaca0
6 changed files with 127 additions and 66 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. 2
      Servo/include/ModbusWrapper.h
  2. 18
      Servo/include/ServoController.h
  3. 10
      Servo/src/ModbusWrapper.cpp
  4. 116
      Servo/src/ServoController.cpp
  5. 13
      Test/MainWindow.cpp
  6. 34
      Test/MainWindow.ui

2
Servo/include/ModbusWrapper.h
View File

@ -21,7 +21,7 @@ public:
~ModbusWrapper(); ~ModbusWrapper();
void connectToDevice(ModbusConfig modbusConfig); void connectToDevice(ModbusConfig modbusConfig);
void init(); void init();
void close();
QBitArray getCoil(int startAddress, quint16 readSize); QBitArray getCoil(int startAddress, quint16 readSize);
QBitArray getInputCoil(int startAddress, quint16 readSize); QBitArray getInputCoil(int startAddress, quint16 readSize);
QVector<quint16> getHoldingRegister(int startAddress, quint16 readSize); QVector<quint16> getHoldingRegister(int startAddress, quint16 readSize);

18
Servo/include/ServoController.h
View File

@ -11,14 +11,14 @@ private:
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;
const int SRV_APPLY_SETTING_REG = 2004; const int SRV_APPLY_SETTING_REG = 2003; //2004;
const int SRV_START_STOP_REG = 2005; const int SRV_START_STOP_REG = 2004; //2005;
const int SRV_ZERO_OFFSET_ANGLE = 2007; const int SRV_ZERO_OFFSET_ANGLE = 2005; //2007;
const int SRV_START_ANGLE_FRAC_PART_REG = 2010; const int SRV_START_ANGLE_FRAC_PART_REG = 2006; //2010;
const int SRV_STOP_ANGLE_FRAC_PART_REG = 2011; const int SRV_STOP_ANGLE_FRAC_PART_REG = 2007; //2011;
const int SRV_CALIBRATE_REG = 2013; const int SRV_CALIBRATE_REG = 2008; //2013;
const int SRV_ENABLE_DRIVE_REG = 2017; const int SRV_ENABLE_DRIVE_REG = 2009; //2017;
const int SRV_AZIMUTH_REG = 2030; const int SRV_AZIMUTH_REG = 2010; //2030;
const double SRV_ANGLE_FACTOR = 100.0; const double SRV_ANGLE_FACTOR = 100.0;
const double SRV_SPEED_FACTOR = 1000.0; const double SRV_SPEED_FACTOR = 1000.0;
@ -48,6 +48,8 @@ public:
double getAngleOffset(); double getAngleOffset();
void setAngleOffset(double offset); void setAngleOffset(double offset);
double getAzimuth(); double getAzimuth();
bool getStatus();
}; };
#endif //SERVOCONTROLLER_H #endif //SERVOCONTROLLER_H

10
Servo/src/ModbusWrapper.cpp
View File

@ -80,7 +80,15 @@ void ModbusWrapper::init()
if(exp.valid) if(exp.valid)
{ {
throw ServoException(exp.message); throw ServoException(exp.message);
} }
}
/*************************************************************************************************/
void ModbusWrapper::close()
{
_modbusMaster.close();
_workerThread.quit();
_workerThread.wait();
} }
/*************************************************************************************************/ /*************************************************************************************************/

116
Servo/src/ServoController.cpp
View File

@ -2,156 +2,170 @@
ModbusConfig ServoController::initiateConfig(QString serialPort) ModbusConfig ServoController::initiateConfig(QString serialPort)
{ {
ModbusConfig _config; ModbusConfig _config;
_config.baud = QSerialPort::Baud19200; _config.baud = QSerialPort::Baud19200;
_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.responseTime = 2000; _config.responseTime = 2000;
_config.serialPort = serialPort; _config.serialPort = serialPort;
_config.clientAddress = 1; _config.clientAddress = 1;
return _config; return _config;
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::apply() 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() void ServoController::enableDrive()
{ {
_modbusWrapper.setSingleRegister(SRV_ENABLE_DRIVE_REG, 3); _modbusWrapper.setSingleRegister(SRV_ENABLE_DRIVE_REG, 3);
apply(); apply();
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::setStartStop(bool start) void ServoController::setStartStop(bool start)
{ {
_modbusWrapper.setSingleRegister(SRV_START_STOP_REG, start ? 3 : 0); _modbusWrapper.setSingleRegister(SRV_START_STOP_REG, start ? 3 : 0);
apply(); apply();
} }
/*************************************************************************************************/ /*************************************************************************************************/
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, _modbusWrapper.setSingleRegister(SRV_START_ANGLE_FRAC_PART_REG,
static_cast<quint16>((angle - (static_cast<int>(angle))) * static_cast<quint16>((angle - (static_cast<int>(angle))) *
100)); 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, _modbusWrapper.setSingleRegister(SRV_STOP_ANGLE_FRAC_PART_REG,
static_cast<quint16>((angle - (static_cast<int>(angle))) * static_cast<quint16>((angle - (static_cast<int>(angle))) *
100)); 100));
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::setSpeed(double speed) void ServoController::setSpeed(double speed)
{ {
_modbusWrapper.setSingleRegister(SRV_AZIMUTH_SPEED_REG, _modbusWrapper.setSingleRegister(SRV_AZIMUTH_SPEED_REG,
static_cast<quint16>(speed * SRV_SPEED_FACTOR)); static_cast<quint16>(speed * SRV_SPEED_FACTOR));
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::changeMode(double startAngle, double stopAngle, double speed) void ServoController::changeMode(double startAngle, double stopAngle, double speed)
{ {
setStartAngle(startAngle); setStartAngle(startAngle);
setStopAngle(stopAngle); setStopAngle(stopAngle);
setSpeed(speed); setSpeed(speed);
setStartStop(true); setStartStop(true);
enableDrive(); enableDrive();
apply(); apply();
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::init(QString serialPort) void ServoController::init(QString serialPort)
{ {
_modbusWrapper.init(); _modbusWrapper.close();
_modbusWrapper.connectToDevice(initiateConfig(serialPort)); _modbusWrapper.init();
_modbusWrapper.connectToDevice(initiateConfig(serialPort));
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::startMoving() void ServoController::startMoving()
{ {
setStartStop(true); setStartStop(true);
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::stopMoving() void ServoController::stopMoving()
{ {
setStartStop(false); setStartStop(false);
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::resetAzimuth() void ServoController::resetAzimuth()
{ {
_modbusWrapper.setSingleRegister(SRV_CALIBRATE_REG, SRV_CALIBRATION_VAL); _modbusWrapper.setSingleRegister(SRV_CALIBRATE_REG, SRV_CALIBRATION_VAL);
} }
/*************************************************************************************************/ /*************************************************************************************************/
void ServoController::fix(double angle) 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) void ServoController::sector(double startAngle, double stopAngle, double speed)
{ {
changeMode(startAngle, stopAngle, speed); changeMode(startAngle, stopAngle, speed);
} }
/*************************************************************************************************/ /*************************************************************************************************/
double ServoController::getSpeed() 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() double ServoController::getStopAngle()
{ {
auto speedVector1 = _modbusWrapper.getHoldingRegister(SRV_STOP_ANGLE_INT_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); 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() double ServoController::getStartAngle()
{ {
auto speedVector1 = _modbusWrapper.getHoldingRegister(SRV_START_ANGLE_INT_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); 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() 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) void ServoController::setAngleOffset(double offset)
{ {
_modbusWrapper.setSingleRegister(SRV_ZERO_OFFSET_ANGLE, _modbusWrapper.setSingleRegister(SRV_ZERO_OFFSET_ANGLE,
static_cast<quint16>(offset * SRV_ANGLE_FACTOR)); static_cast<quint16>(offset * SRV_ANGLE_FACTOR));
} }
/*************************************************************************************************/ /*************************************************************************************************/
double ServoController::getAzimuth() 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;
}
} }

13
Test/MainWindow.cpp
View File

@ -212,15 +212,18 @@ void MainWindow::on_init_clicked()
void MainWindow::handleGetRequestFromServo() void MainWindow::handleGetRequestFromServo()
{ {
try { try {
ui->showSpeed->setText(QString::number(_servoControler.getSpeed())); // ui->showSpeed->setText(QString::number(_servoControler.getSpeed()));
ui->showStartAngle->setText(QString::number(_servoControler.getStartAngle())); //ui->showStartAngle->setText(QString::number(_servoControler.getStartAngle()));
ui->showStopAngle->setText(QString::number(_servoControler.getStopAngle())); //ui->showStopAngle->setText(QString::number(_servoControler.getStopAngle()));
ui->showAngleOffset->setText(QString::number(_servoControler.getAngleOffset())); //ui->showAngleOffset->setText(QString::number(_servoControler.getAngleOffset()));
ui->showAzimuth->setText(QString::number(_servoControler.getAzimuth())); //ui->showAzimuth->setText(QString::number(_servoControler.getAzimuth()));
ui->connectionState->setText(QVariant(_servoControler.getStatus()).toString());
} }
catch(ServoException exp) catch(ServoException exp)
{ {
timer->stop();
ui->showServoError->setText(exp.getMessage()); ui->showServoError->setText(exp.getMessage());
} }
} }

34
Test/MainWindow.ui
View File

@ -621,6 +621,40 @@ background-color: rgb(186, 178, 158);</string>
</rect> </rect>
</property> </property>
</widget> </widget>
<widget class="QLabel" name="label_18">
<property name="geometry">
<rect>
<x>180</x>
<y>0</y>
<width>181</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Connection State to Client:</string>
</property>
</widget>
<widget class="QLabel" name="connectionState">
<property name="geometry">
<rect>
<x>370</x>
<y>0</y>
<width>121</width>
<height>20</height>
</rect>
</property>
<property name="styleSheet">
<string notr="true">background-color: rgb(52, 101, 164);
color: rgb(238, 238, 236);
</string>
</property>
<property name="text">
<string/>
</property>
<property name="margin">
<number>0</number>
</property>
</widget>
</widget> </widget>
<widget class="QGroupBox" name="groupBox_12"> <widget class="QGroupBox" name="groupBox_12">
<property name="geometry"> <property name="geometry">

Write Preview
Loading…
Cancel
Save