Skip to content

Commit

Permalink
Add support for PWM servo mode and PWM capture mode.
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@danielsheh02
danielsheh02 committed Dec 22, 2024
1 parent 738c352 commit 541c167
Show file tree
Hide file tree
Showing 8 changed files with 84 additions and 27 deletions.
20 changes: 13 additions & 7 deletions tests/kernel-3.6/test-system-config.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
/>
<servoMotor
port="S2"
deviceFile="/sys/class/pwm/ecap.1/duty_ns"
periodFile="/sys/class/pwm/ecap.1/period_ns"
runFile="/sys/class/pwm/ecap.1/run"
captureFile="/sys/class/pwm/ecap.1/capture"
/>
<servoMotor
port="S3"
deviceFile="/sys/class/pwm/ecap.0/duty_ns"
periodFile="/sys/class/pwm/ecap.0/period_ns"
runFile="/sys/class/pwm/ecap.0/run"
captureFile="/sys/class/pwm/ecap.0/capture"
/>
<servoMotor
port="S4"
Expand All @@ -113,18 +116,21 @@ equal to its class name.
/>
<pwmCapture
port="S1"
frequencyFile="/sys/class/pwm/ecap_cap.2/freq"
dutyFile="/sys/class/pwm/ecap_cap.2/duty_percentage"
frequencyFile="/sys/class/pwm/ecap.2/period_freq"
dutyFile="/sys/class/pwm/ecap.2/duty_ns"
captureFile="/sys/class/pwm/ecap.2/capture"
/>
<pwmCapture
port="S2"
frequencyFile="/sys/class/pwm/ecap_cap.1/freq"
dutyFile="/sys/class/pwm/ecap_cap.1/duty_percentage"
frequencyFile="/sys/class/pwm/ecap.1/period_freq"
dutyFile="/sys/class/pwm/ecap.1/duty_ns"
captureFile="/sys/class/pwm/ecap.1/capture"
/>
<pwmCapture
port="S3"
frequencyFile="/sys/class/pwm/ecap_cap.0/freq"
dutyFile="/sys/class/pwm/ecap_cap.0/duty_percentage"
port="S3"
frequencyFile="/sys/class/pwm/ecap.0/period_freq"
dutyFile="/sys/class/pwm/ecap.0/duty_ns"
captureFile="/sys/class/pwm/ecap.0/capture"
/>
<powerMotor port="M1" i2cCommandNumber="0x14" invert="true" />
<powerMotor port="M2" i2cCommandNumber="0x15" />
Expand Down
6 changes: 6 additions & 0 deletions tests/kernel-4.14/test-system-config.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
/>
<servoMotor
port="S2"
deviceFile="/sys/class/pwm/ecap.1/duty_ns"
periodFile="/sys/class/pwm/ecap.1/period_ns"
runFile="/sys/class/pwm/ecap.1/run"
captureFile="/sys/class/pwm/ecap.1/capture"
/>
<servoMotor
port="S3"
deviceFile="/sys/class/pwm/ecap.0/duty_ns"
periodFile="/sys/class/pwm/ecap.0/period_ns"
runFile="/sys/class/pwm/ecap.0/run"
captureFile="/sys/class/pwm/ecap.0/capture"
/>
<servoMotor
port="S4"
Expand All @@ -133,16 +136,19 @@ equal to its class name.
port="S1"
frequencyFile="/sys/class/pwm/ecap_cap.2/freq"
dutyFile="/sys/class/pwm/ecap_cap.2/duty_percentage"
captureFile="/sys/class/pwm/ecap.2/capture"
/>
<pwmCapture
port="S2"
frequencyFile="/sys/class/pwm/ecap_cap.1/freq"
dutyFile="/sys/class/pwm/ecap_cap.1/duty_percentage"
captureFile="/sys/class/pwm/ecap.1/capture"
/>
<pwmCapture
port="S3"
frequencyFile="/sys/class/pwm/ecap_cap.0/freq"
dutyFile="/sys/class/pwm/ecap_cap.0/duty_percentage"
captureFile="/sys/class/pwm/ecap.0/capture"
/>
<powerMotor port="M1" i2cCommandNumber="0x14" invert="true" />
<powerMotor port="M2" i2cCommandNumber="0x15" />
Expand Down
20 changes: 13 additions & 7 deletions trikControl/configs/kernel-3.6/system-config.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
/>
<servoMotor
port="S2"
deviceFile="/sys/class/pwm/ecap.1/duty_ns"
periodFile="/sys/class/pwm/ecap.1/period_ns"
runFile="/sys/class/pwm/ecap.1/run"
captureFile="/sys/class/pwm/ecap.1/capture"
/>
<servoMotor
port="S3"
deviceFile="/sys/class/pwm/ecap.0/duty_ns"
periodFile="/sys/class/pwm/ecap.0/period_ns"
runFile="/sys/class/pwm/ecap.0/run"
captureFile="/sys/class/pwm/ecap.0/capture"
/>
<servoMotor
port="S4"
Expand All @@ -115,18 +118,21 @@ equal to its class name.
/>
<pwmCapture
port="S1"
frequencyFile="/sys/class/pwm/ecap_cap.2/period_freq"
dutyFile="/sys/class/pwm/ecap_cap.2/duty_ns"
frequencyFile="/sys/class/pwm/ecap.2/period_freq"
dutyFile="/sys/class/pwm/ecap.2/duty_ns"
captureFile="/sys/class/pwm/ecap.2/capture"
/>
<pwmCapture
port="S2"
frequencyFile="/sys/class/pwm/ecap_cap.1/period_freq"
dutyFile="/sys/class/pwm/ecap_cap.1/duty_ns"
frequencyFile="/sys/class/pwm/ecap.1/period_freq"
dutyFile="/sys/class/pwm/ecap.1/duty_ns"
captureFile="/sys/class/pwm/ecap.1/capture"
/>
<pwmCapture
port="S3"
frequencyFile="/sys/class/pwm/ecap_cap.0/period_freq"
dutyFile="/sys/class/pwm/ecap_cap.0/duty_ns"
port="S3"
frequencyFile="/sys/class/pwm/ecap.0/period_freq"
dutyFile="/sys/class/pwm/ecap.0/duty_ns"
captureFile="/sys/class/pwm/ecap.0/capture"
/>
<powerMotor port="M1" i2cCommandNumber="0x14" invert="true" />
<powerMotor port="M2" i2cCommandNumber="0x15" />
Expand Down
6 changes: 6 additions & 0 deletions trikControl/configs/kernel-4.14/system-config.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
/>
<servoMotor
port="S2"
deviceFile="/sys/class/pwm/pwmchip1/pwm0/duty_cycle"
periodFile="/sys/class/pwm/pwmchip1/pwm0/period"
runFile="/sys/class/pwm/pwmchip1/pwm0/enable"
captureFile="/sys/class/pwm/pwmchip1/pwm0/capture"
/>
<servoMotor
port="S3"
deviceFile="/sys/class/pwm/pwmchip0/pwm0/duty_cycle"
periodFile="/sys/class/pwm/pwmchip0/pwm0/period"
runFile="/sys/class/pwm/pwmchip0/pwm0/enable"
captureFile="/sys/class/pwm/pwmchip0/pwm0/capture"
/>
<servoMotor
port="S4"
Expand All @@ -132,16 +135,19 @@ equal to its class name.
port="S1"
frequencyFile="/sys/class/pwm/ecap_cap.2/freq"
dutyFile="/sys/class/pwm/ecap_cap.2/duty_percentage"
captureFile="/sys/class/pwm/ecap_cap.2/capture"
/>
<pwmCapture
port="S2"
frequencyFile="/sys/class/pwm/ecap_cap.1/freq"
dutyFile="/sys/class/pwm/ecap_cap.1/duty_percentage"
captureFile="/sys/class/pwm/ecap_cap.1/capture"
/>
<pwmCapture
port="S3"
frequencyFile="/sys/class/pwm/ecap_cap.0/freq"
dutyFile="/sys/class/pwm/ecap_cap.0/duty_percentage"
captureFile="/sys/class/pwm/ecap_cap.0/capture"
/>
<powerMotor port="M1" i2cCommandNumber="0x14" invert="true" />
<powerMotor port="M2" i2cCommandNumber="0x15" />
Expand Down
8 changes: 8 additions & 0 deletions trikControl/src/pwmCapture.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<trikHal::OutputDeviceFileInterface> captureFile
(hardwareAbstraction.createOutputDeviceFile(configurer.attributeByPort(port, "captureFile")));
if (!captureFile->open()) {
mState.fail();
return;
}
captureFile->write(QString::number(1));

if (!mFrequencyFile->open()) {
mState.fail();
}
Expand Down
28 changes: 21 additions & 7 deletions trikControl/src/servoMotor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<trikHal::OutputDeviceFileInterface> captureFile
(hardwareAbstraction.createOutputDeviceFile(captureFileAttribute));
if (!captureFile->open()) {
mState.fail();
return;
}
captureFile->write(QString::number(0));
}

const auto configure = [this, &port, &configurer](const QString &parameterName) {
return ConfigurerHelper::configureInt(configurer, mState, port, parameterName);
};
Expand All @@ -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()
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
}
5 changes: 4 additions & 1 deletion trikKernel/include/trikKernel/configurer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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).
Expand Down
18 changes: 13 additions & 5 deletions trikKernel/src/configurer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand Down Expand Up @@ -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));
}
Expand All @@ -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));
}

Expand Down

0 comments on commit 541c167

Please sign in to comment.