Merge pull request #51 from romankurbatov/encodersInDegrees

Encoders in degrees

Author: yurii-litvinov

trikControl/config.xml

@@ -145,9 +145,9 @@ Use it if JCx ports are configured to work as PWM signal generators.
 	<!-- Encoders configuration, maps logical port to I2C command.
 		 I2C device path and device id are set separately, in "i2c" section. -->
 	<encoders>
-		<encoder port="B2" i2cCommandNumber="0x31" />
-		<encoder port="B4" i2cCommandNumber="0x32" />
-		<encoder port="B3" i2cCommandNumber="0x33" />
+		<encoder port="B2" i2cCommandNumber="0x31" defaultType="defaultEncoder"/>
+		<encoder port="B4" i2cCommandNumber="0x32" defaultType="defaultEncoder"/>
+		<encoder port="B3" i2cCommandNumber="0x33" defaultType="defaultEncoder"/>
 	</encoders>
 
 	<!-- Description of servo motor types used in servo motors mapping. Supplied values correspond to
@@ -182,6 +182,11 @@ Use it if JCx ports are configured to work as PWM signal generators.
 		<volumeSensor min="0" max="100"/>
 	</digitalSensorTypes>
 
+	<!-- Description of encoder types. Provides a coefficient for converting raw encoder values to degrees. -->
+	<encoderTypes>
+		<defaultEncoder rawToDegrees="0.02304708" />
+	</encoderTypes>
+
 	<!-- Format for playSound command, used to play .wav files. %1 designates file name to be played. -->
 	<playWavFile command="aplay --quiet &quot;%1&quot; &amp;" />
 

trikControl/config_capture.xml

@@ -182,6 +182,11 @@ Use it if JCx ports are configured to work as PWM signal capture devices.
 		<volumeSensor min="0" max="100"/>
 	</digitalSensorTypes>
 
+	<!-- Description of encoder types. Provides a coefficient for converting raw encoder values to degrees. -->
+	<encoderTypes>
+		<defaultEncoder rawToDegrees="0.02304708" />
+	</encoderTypes>
+
 	<!-- Format for playSound command, used to play .wav files. %1 designates file name to be played. -->
 	<playWavFile command="aplay --quiet &quot;%1&quot; &amp;" />
 

trikControl/include/trikControl/encoder.h

@@ -31,18 +31,20 @@ class TRIKCONTROL_EXPORT Encoder : public QObject
 	/// Constructor.
 	/// @param communicator - I2C communicator.
 	/// @param i2cCommandNumber - number of I2C command to query this encoder.
-	Encoder(I2cCommunicator &communicator, int i2cCommandNumber);
+	/// @param rawToDegrees - coefficient for converting raw encoder readings to degrees.
+	Encoder(I2cCommunicator &communicator, int i2cCommandNumber, double rawToDegrees);
 
 public slots:
-	/// Returns current encoder reading.
-	float read();
+	/// Returns current encoder reading (in degrees).
+	int read();
 
 	/// Resets encoder by setting current reading to 0.
 	void reset();
 
 private:
 	I2cCommunicator &mCommunicator;
 	int mI2cCommandNumber;
+	double mRawToDegrees;
 };
 
 }

trikControl/src/brick.cpp

@@ -119,7 +119,12 @@ Brick::Brick(QThread &guiThread, QString const &configFilePath, const QString &s
 	}
 
 	for (QString const &port : mConfigurer->encoderPorts()) {
-		Encoder *encoder = new Encoder(*mI2cCommunicator, mConfigurer->encoderI2cCommandNumber(port));
+		QString const encoderType = mConfigurer->encoderDefaultType(port);
+
+		Encoder *encoder = new Encoder(
+				*mI2cCommunicator
+				, mConfigurer->encoderI2cCommandNumber(port)
+				, mConfigurer->encoderTypeRawToDegrees(encoderType));
 		mEncoders.insert(port, encoder);
 	}
 

trikControl/src/configurer.cpp

@@ -52,6 +52,7 @@ Configurer::Configurer(QString const &configFilePath)
 	loadServoMotorTypes(root);
 	loadAnalogSensorTypes(root);
 	loadDigitalSensorTypes(root);
+	loadEncoderTypes(root);
 	loadSound(root);
 
 	mAccelerometer = loadSensor3d(root, "accelerometer");
@@ -141,6 +142,11 @@ int Configurer::digitalSensorTypeMax(QString const &digitalSensorType) const
 	return mDigitalSensorTypes[digitalSensorType].max;
 }
 
+double Configurer::encoderTypeRawToDegrees(QString const &encoderType) const
+{
+	return mEncoderTypes[encoderType].rawToDegrees;
+}
+
 QStringList Configurer::servoMotorPorts() const
 {
 	return mServoMotorMappings.keys();
@@ -226,6 +232,11 @@ int Configurer::encoderI2cCommandNumber(QString const &port) const
 	return mEncoderMappings[port].i2cCommandNumber;
 }
 
+QString Configurer::encoderDefaultType(QString const &port) const
+{
+	return mEncoderMappings[port].defaultType;
+}
+
 QStringList Configurer::digitalSensorPorts() const
 {
 	return mDigitalSensorMappings.keys();
@@ -579,6 +590,7 @@ void Configurer::loadEncoders(QDomElement const &root)
 		EncoderMapping mapping;
 		mapping.port = childElement.attribute("port");
 		mapping.i2cCommandNumber = childElement.attribute("i2cCommandNumber").toInt(NULL, 0);
+		mapping.defaultType = childElement.attribute("defaultType");
 
 		mEncoderMappings.insert(mapping.port, mapping);
 	}
@@ -700,6 +712,31 @@ void Configurer::loadDigitalSensorTypes(QDomElement const &root)
 	}
 }
 
+void Configurer::loadEncoderTypes(const QDomElement &root)
+{
+	if (root.elementsByTagName("encoderTypes").isEmpty()) {
+		qDebug() << "config.xml does not have <encoderTypes> tag";
+		throw "config.xml parsing failed";
+	}
+
+	QDomElement const encoderTypes = root.elementsByTagName("encoderTypes").at(0).toElement();
+	for (QDomNode child = encoderTypes.firstChild()
+			; !child.isNull()
+			; child = child.nextSibling())
+	{
+		if (!child.isElement()) {
+			continue;
+		}
+
+		QDomElement const childElement = child.toElement();
+		QString const typeName = childElement.nodeName();
+
+		EncoderType encoderType;
+		encoderType.rawToDegrees = childElement.attribute("rawToDegrees").toDouble();
+		mEncoderTypes.insert(typeName, encoderType);
+	}
+}
+
 void Configurer::loadSound(QDomElement const &root)
 {
 	if (root.elementsByTagName("playWavFile").isEmpty()) {

trikControl/src/configurer.h

@@ -69,6 +69,8 @@ class Configurer {
 
 	int digitalSensorTypeMax(QString const &digitalSensorType) const;
 
+	double encoderTypeRawToDegrees(QString const &encoderType) const;
+
 	QStringList servoMotorPorts() const;
 
 	QString servoMotorDeviceFile(QString const &port) const;
@@ -103,6 +105,8 @@ class Configurer {
 
 	int encoderI2cCommandNumber(QString const &port) const;
 
+	QString encoderDefaultType(QString const &port) const;
+
 	QStringList digitalSensorPorts() const;
 
 	QString digitalSensorDeviceFile(QString const &port) const;
@@ -205,6 +209,10 @@ class Configurer {
 		int max;
 	};
 
+	struct EncoderType {
+		double rawToDegrees;
+	};
+
 	struct ServoMotorMapping {
 		QString port;
 		QString deviceFile;
@@ -235,6 +243,7 @@ class Configurer {
 	struct EncoderMapping {
 		QString port;
 		int i2cCommandNumber;
+		QString defaultType;
 	};
 
 	struct DigitalSensorMapping {
@@ -268,6 +277,7 @@ class Configurer {
 	void loadServoMotorTypes(QDomElement const &root);
 	void loadAnalogSensorTypes(QDomElement const &root);
 	void loadDigitalSensorTypes(QDomElement const &root);
+	void loadEncoderTypes(QDomElement const &root);
 	void loadSound(QDomElement const &root);
 	static OnBoardSensor loadSensor3d(QDomElement const &root, QString const &tagName);
 	void loadI2c(QDomElement const &root);
@@ -281,6 +291,7 @@ class Configurer {
 	QHash<QString, ServoMotorType> mServoMotorTypes;
 	QHash<QString, AnalogSensorType> mAnalogSensorTypes;
 	QHash<QString, DigitalSensorType> mDigitalSensorTypes;
+	QHash<QString, EncoderType> mEncoderTypes;
 	QHash<QString, ServoMotorMapping> mServoMotorMappings;
 	QHash<QString, PwmCaptureMapping> mPwmCaptureMappings;
 	QHash<QString, PowerMotorMapping> mPowerMotorMappings;

trikControl/src/encoder.cpp

@@ -16,13 +16,12 @@
 
 #include "src/i2cCommunicator.h"
 
-float const parToRad = 0.03272492;
-
 using namespace trikControl;
 
-Encoder::Encoder(I2cCommunicator &communicator, int i2cCommandNumber)
+Encoder::Encoder(I2cCommunicator &communicator, int i2cCommandNumber, double rawToDegrees)
 	: mCommunicator(communicator)
 	, mI2cCommandNumber(i2cCommandNumber)
+	, mRawToDegrees(rawToDegrees)
 {
 }
 
@@ -35,11 +34,11 @@ void Encoder::reset()
 	mCommunicator.send(command);
 }
 
-float Encoder::read()
+int Encoder::read()
 {
 	QByteArray command(2, '\0');
 	command[0] = static_cast<char>(mI2cCommandNumber);
 	int data = mCommunicator.read(command);
 
-	return parToRad * data;
+	return mRawToDegrees * data;
 }