From 541c167885b1b5297a88b5847b2098737747dd12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Chehade=20Daniel=20=28=D0=A8=D0=B5=D1=85=D0=B0=D0=B4=D0=B5?= =?UTF-8?q?=20=D0=94=D0=B0=D0=BD=D0=B8=D1=8D=D0=BB=D1=8C=29?= <71555323+danielsheh02@users.noreply.github.com> Date: Mon, 9 Dec 2024 14:40:59 +0300 Subject: [PATCH] Add support for PWM servo mode and PWM capture mode. For ports S1, S2, S3, if they are in servo mode, it is necessary to write to captureFile 0. For capture mode, you need to write to captureFile 1. captureFile is not needed for ports S4, S5, S6. --- tests/kernel-3.6/test-system-config.xml | 20 ++++++++----- tests/kernel-4.14/test-system-config.xml | 6 ++++ .../configs/kernel-3.6/system-config.xml | 20 ++++++++----- .../configs/kernel-4.14/system-config.xml | 6 ++++ trikControl/src/pwmCapture.cpp | 8 ++++++ trikControl/src/servoMotor.cpp | 28 ++++++++++++++----- trikKernel/include/trikKernel/configurer.h | 5 +++- trikKernel/src/configurer.cpp | 18 ++++++++---- 8 files changed, 84 insertions(+), 27 deletions(-) diff --git a/tests/kernel-3.6/test-system-config.xml b/tests/kernel-3.6/test-system-config.xml index a6c5bc749..5a96d9ca4 100644 --- a/tests/kernel-3.6/test-system-config.xml +++ b/tests/kernel-3.6/test-system-config.xml @@ -78,18 +78,21 @@ equal to its class name. deviceFile="/sys/class/pwm/ecap.2/duty_ns" periodFile="/sys/class/pwm/ecap.2/period_ns" runFile="/sys/class/pwm/ecap.2/run" + captureFile="/sys/class/pwm/ecap.2/capture" /> diff --git a/tests/kernel-4.14/test-system-config.xml b/tests/kernel-4.14/test-system-config.xml index 3a293f0d9..27d0ea120 100644 --- a/tests/kernel-4.14/test-system-config.xml +++ b/tests/kernel-4.14/test-system-config.xml @@ -96,18 +96,21 @@ equal to its class name. deviceFile="/sys/class/pwm/ecap.2/duty_ns" periodFile="/sys/class/pwm/ecap.2/period_ns" runFile="/sys/class/pwm/ecap.2/run" + captureFile="/sys/class/pwm/ecap.2/capture" /> diff --git a/trikControl/configs/kernel-3.6/system-config.xml b/trikControl/configs/kernel-3.6/system-config.xml index e32437f6d..13c5d8383 100644 --- a/trikControl/configs/kernel-3.6/system-config.xml +++ b/trikControl/configs/kernel-3.6/system-config.xml @@ -80,18 +80,21 @@ equal to its class name. deviceFile="/sys/class/pwm/ecap.2/duty_ns" periodFile="/sys/class/pwm/ecap.2/period_ns" runFile="/sys/class/pwm/ecap.2/run" + captureFile="/sys/class/pwm/ecap.2/capture" /> diff --git a/trikControl/configs/kernel-4.14/system-config.xml b/trikControl/configs/kernel-4.14/system-config.xml index 41ec35f88..3509d6b3e 100644 --- a/trikControl/configs/kernel-4.14/system-config.xml +++ b/trikControl/configs/kernel-4.14/system-config.xml @@ -95,18 +95,21 @@ equal to its class name. deviceFile="/sys/class/pwm/pwmchip2/pwm0/duty_cycle" periodFile="/sys/class/pwm/pwmchip2/pwm0/period" runFile="/sys/class/pwm/pwmchip2/pwm0/enable" + captureFile="/sys/class/pwm/pwmchip2/pwm0/capture" /> diff --git a/trikControl/src/pwmCapture.cpp b/trikControl/src/pwmCapture.cpp index 8fe39c524..fb1efcfe8 100644 --- a/trikControl/src/pwmCapture.cpp +++ b/trikControl/src/pwmCapture.cpp @@ -33,6 +33,14 @@ PwmCapture::PwmCapture(const QString &port, const trikKernel::Configurer &config , mDutyFile(hardwareAbstraction.createInputDeviceFile(configurer.attributeByPort(port, "dutyFile"))) , mState("PWM Capture on " + port) { + QScopedPointer captureFile + (hardwareAbstraction.createOutputDeviceFile(configurer.attributeByPort(port, "captureFile"))); + if (!captureFile->open()) { + mState.fail(); + return; + } + captureFile->write(QString::number(1)); + if (!mFrequencyFile->open()) { mState.fail(); } diff --git a/trikControl/src/servoMotor.cpp b/trikControl/src/servoMotor.cpp index da3d06645..f81c34f34 100644 --- a/trikControl/src/servoMotor.cpp +++ b/trikControl/src/servoMotor.cpp @@ -33,6 +33,18 @@ ServoMotor::ServoMotor(const QString &port, const trikKernel::Configurer &config , mRun(false) , mState("Servomotor on " + port) { + QString captureFileAttribute = ""; + captureFileAttribute = configurer.attributeByPort(port, "captureFile", &captureFileAttribute); + if (!captureFileAttribute.isEmpty()) { + QScopedPointer captureFile + (hardwareAbstraction.createOutputDeviceFile(captureFileAttribute)); + if (!captureFile->open()) { + mState.fail(); + return; + } + captureFile->write(QString::number(0)); + } + const auto configure = [this, &port, &configurer](const QString ¶meterName) { return ConfigurerHelper::configureInt(configurer, mState, port, parameterName); }; @@ -57,13 +69,16 @@ ServoMotor::ServoMotor(const QString &port, const trikKernel::Configurer &config mRunFile->write(QString::number(mRun ? 1 : 0)); } - setPeriod(mPeriod / 1000); if (!mDutyFile->open()) { mState.fail(); + return; } else { + mDutyFile->write(QString::number(0)); mState.ready(); } + + setPeriod(mPeriod / 1000); } ServoMotor::~ServoMotor() @@ -107,13 +122,10 @@ void ServoMotor::powerOff() return; } - mDutyFile->write(QString::number(mStop)); mRunFile->write(QString::number(0)); + mDutyFile->write(QString::number(mStop)); mRun = false; mCurrentPower = 0; - - mRun = false; - mRunFile->write(QString::number(mRun)); } void ServoMotor::setPeriod(int uSec) @@ -151,10 +163,12 @@ void ServoMotor::setPower(int power, bool constrain) mCurrentDutyPercent = 100 * duty / mPeriod; - mDutyFile->write(command); - if (!mRun) { mRun = true; mRunFile->write(QString::number(mRun)); } + + // After switching from capture mode to servo mode, + // a faster response occurs if you set duty after run. + mDutyFile->write(command); } diff --git a/trikKernel/include/trikKernel/configurer.h b/trikKernel/include/trikKernel/configurer.h index 1362f8e98..e8117ee09 100644 --- a/trikKernel/include/trikKernel/configurer.h +++ b/trikKernel/include/trikKernel/configurer.h @@ -37,8 +37,11 @@ class TRIKKERNEL_EXPORT Configurer /// Returns value of given attribute of given device. QString attributeByDevice(const QString &deviceClass, const QString &attributeName) const; + /// Checks if there is a desired default value or if an exception can be thrown. + QString getDefaultOrException(QString *defaultValue, const QString &error) const; + /// Returns value of given attribute of a device on given port. - QString attributeByPort(const QString &port, const QString &attributeName) const; + QString attributeByPort(const QString &port, const QString &attributeName, QString *defaultValue = nullptr) const; /// Returns true if device is enabled in current configuration (either explicitly enabled in model configuration /// or can not be disabled at all). diff --git a/trikKernel/src/configurer.cpp b/trikKernel/src/configurer.cpp index 08699841c..8ddc2d737 100644 --- a/trikKernel/src/configurer.cpp +++ b/trikKernel/src/configurer.cpp @@ -79,10 +79,18 @@ QString Configurer::attributeByDevice(const QString &deviceClass, const QString QString("Unknown attribute '%1' of device '%2'").arg(attributeName).arg(deviceClass)); } -QString Configurer::attributeByPort(const QString &port, const QString &attributeName) const +QString Configurer::getDefaultOrException(QString *defaultValue, const QString &error) const +{ + if (defaultValue){ + return *defaultValue; + } + throw MalformedConfigException(error); +} + +QString Configurer::attributeByPort(const QString &port, const QString &attributeName, QString *defaultValue) const { if (!mModelConfiguration.contains(port)) { - throw MalformedConfigException(QString("Port '%1' is not configured").arg(port)); + return getDefaultOrException(defaultValue, QString("Port '%1' is not configured").arg(port)); } if (mModelConfiguration[port].attributes.contains(attributeName)) { @@ -110,11 +118,11 @@ QString Configurer::attributeByPort(const QString &port, const QString &attribut } if (!device.portSpecificAttributes.contains(port)) { - throw MalformedConfigException(QString("Device type '%1' is not allowed on port %2.") + return getDefaultOrException(defaultValue, QString("Device type '%1' is not allowed on port %2.") .arg(deviceType).arg(port)); } } else { - throw MalformedConfigException( + return getDefaultOrException(defaultValue, QString("Device type '%1' has device class '%2' which is not listed in 'deviceClasses' section.") .arg(deviceType).arg(deviceClass)); } @@ -133,7 +141,7 @@ QString Configurer::attributeByPort(const QString &port, const QString &attribut } } - throw MalformedConfigException(QString("Unknown attribute '%1' of device '%2' on port '%3'") + return getDefaultOrException(defaultValue, QString("Unknown attribute '%1' of device '%2' on port '%3'") .arg(attributeName).arg(deviceType).arg(port)); }