Merge pull request #103 from Kseniya-Gonta/Gonta

Added calibration of IR sensor

Author: yurii-litvinov

.gitignore

@@ -1,3 +1,4 @@
+*~
 # Compiled Object files
 *.slo
 *.lo

trikControl/model-config.xml

@@ -46,22 +46,22 @@ Note that device, even if listed here, may fail by itself, then it also will not
 
 	<!-- Analog sensor ports. -->
 	<A1>
-		<irSensor />
+		<sharpGP2Sensor />
 	</A1>
 	<A2>
-		<irSensor />
+		<sharpGP2Sensor />
 	</A2>
 	<A3>
-		<irSensor />
+		<sharpGP2Sensor />
 	</A3>
 	<A4>
-		<irSensor />
+		<analogSensor />
 	</A4>
 	<A5>
-		<irSensor />
+		<analogSensor />
 	</A5>
 	<A6>
-		<irSensor />
+		<analogSensor />
 	</A6>
 
 	<!-- Digital sensor ports. -->

trikControl/src/analogSensor.cpp

@@ -1,4 +1,4 @@
-/* Copyright 2013 - 2015 Nikita Batov and CyberTech Labs Ltd.
+/* Copyright 2013 - 2015 Nikita Batov, Kseniya Gonta and CyberTech Labs Ltd.
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -26,26 +26,20 @@ AnalogSensor::AnalogSensor(const QString &port, const trikKernel::Configurer &co
 	: mCommunicator(communicator)
 {
 	mI2cCommandNumber = ConfigurerHelper::configureInt(configurer, mState, port, "i2cCommandNumber");
-
-	// We use linear subjection to normalize sensor values:
+	mIRType = configurer.attributeByPort(port, "type") == "SharpGP2" ? Type::sharpGP2 : Type::analog;
+	// We use linear subjection to normalize common analog sensor values:
 	// normalizedValue = k * rawValue + b
 	// To calculate k and b we need two raw values and two corresponding them normalized values.
 
-	const int rawValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue1");
-	const int rawValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue2");
-	const int normalizedValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue1");
-	const int normalizedValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue2");
+	// We use hyperbolical subjection to normalize SharpGP2 sensor values:
+	// normalizedValue = s / (rawValue + l) + n
+	// To calculate s, l and n we need sensor readings at three distances.
 
-	if (rawValue1 == rawValue2) {
-		QLOG_ERROR() <<  "Sensor calibration error: rawValue1 = rawValue2!";
-		mState.fail();
-		mK = 0;
-		mB = 0;
+	if (mIRType == Type::sharpGP2){
+		calculateLNS(port, configurer);	
 	} else {
-		mK = static_cast<qreal>(normalizedValue2 - normalizedValue1) / (rawValue2 - rawValue1);
-		mB = normalizedValue1 - mK * rawValue1;
+		calculateKB(port, configurer);	
 	}
-
 	mState.ready();
 }
 
@@ -56,14 +50,11 @@ AnalogSensor::Status AnalogSensor::status() const
 
 int AnalogSensor::read()
 {
-	if (!mState.isReady() || mCommunicator.status() != DeviceInterface::Status::ready) {
-		return 0;
+	const auto raw = readRawData();
+	if (mIRType == Type::sharpGP2) {
+		return mS /(raw + mL) + mN;	
 	}
-
-	QByteArray command(1, '\0');
-	command[0] = static_cast<char>(mI2cCommandNumber & 0xFF);
-
-	return mK * mCommunicator.read(command) + mB;
+	return mK * raw + mB;
 }
 
 int AnalogSensor::readRawData()
@@ -77,3 +68,41 @@ int AnalogSensor::readRawData()
 
 	return mCommunicator.read(command);
 }
+
+void AnalogSensor::calculateLNS(const QString &port, const trikKernel::Configurer &configurer)
+{
+	QStringList result;
+	for (const QString &str : configurer.attributeByPort(port, "values").split(")")) {
+		result += str.mid(1).split(";");
+	}
+
+	const int x1 = result[0].toInt();
+	const int y1 = result[1].toInt();
+	const int x2 = result[2].toInt();
+	const int y2 = result[3].toInt();
+	const int x3 = result[4].toInt();
+	const int y3 = result[5].toInt();
+
+	// Counted from equations x1 = mS/(y1 + mL) + mN, x2 = mS/(y2 + mL) + mN, x3 = mS/(y3 + mL) + mN 
+	mL = (-x1 * y1 * y3 - x3 * y2 * y3 + x3 * y1 * y3 + x1 * y1 * y2 + x2 * y2 * y3 - x2 * y1 * y2) / 
+			(x1 * y3 - x2 * y3 + x2 * y1 - x1 * y2 + x3 * y2 - x3 * y1);
+	mS = (x1 - x2) * (y1 + mL) * (y2 + mL) / (y2 - y1);
+	mN = x1 - mS / (y1 + mL);	
+}
+
+void AnalogSensor::calculateKB(const QString &port, const trikKernel::Configurer &configurer)
+{
+	const int rawValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue1");
+	const int rawValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "rawValue2");
+	const int normalizedValue1 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue1");
+	const int normalizedValue2 = ConfigurerHelper::configureInt(configurer, mState, port, "normalizedValue2");
+	if (rawValue1 == rawValue2) {
+		QLOG_ERROR() <<  "Sensor calibration error: rawValue1 = rawValue2!";
+		mState.fail();
+		mK = 0;
+		mB = 0;
+	} else {
+		mK = static_cast<qreal>(normalizedValue2 - normalizedValue1) / (rawValue2 - rawValue1);
+		mB = normalizedValue1 - mK * rawValue1;
+	}
+}

trikControl/src/analogSensor.h

@@ -1,4 +1,4 @@
-/* Copyright 2013 - 2015 Nikita Batov and CyberTech Labs Ltd.
+/* Copyright 2013 - 2015 Nikita Batov, Kseniya Gonta and CyberTech Labs Ltd.
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -50,15 +50,40 @@ public slots:
 	int readRawData() override;
 
 private:
+	enum class Type 
+	{
+		/// Calibrated IR sensor.
+		sharpGP2
+
+		/// Normalized IR sensor.
+		, analog	
+	};
+	
+	void calculateLNS(const QString &port, const trikKernel::Configurer &configurer);
+	void calculateKB(const QString &port, const trikKernel::Configurer &configurer);
+
 	I2cCommunicator &mCommunicator;
 	int mI2cCommandNumber = 0;
+	Type mIRType;
 
 	/// Linear approximation coefficient k. Normalized value is calculated as normalizedValue = k * rawValue + b.
 	qreal mK = 0;
 
 	/// Linear approximation coefficient b. Normalized value is calculated as normalizedValue = k * rawValue + b.
 	qreal mB = 0;
 
+	/// Hyperbolical approximation coefficient l.
+	/// Normalized value is calculated as normalizedValue = s / (rawValue + l) + n.
+	int mL = 0;
+
+	/// Hyperbolical approximation coefficient n. 
+	/// Normalized value is calculated as normalizedValue = s / (rawValue + l) + n.
+	int mN = 0;
+
+	/// Hyperbolical approximation coefficient s.
+	/// Normalized value is calculated as normalizedValue = s / (rawValue + l) + n.
+	int mS = 0;
+	
 	/// State of a device.
 	DeviceState mState;
 };

trikControl/src/powerMotor.cpp

@@ -25,6 +25,7 @@ PowerMotor::PowerMotor(const QString &port, const trikKernel::Configurer &config
 	: mCommunicator(communicator)
 	, mInvert(configurer.attributeByPort(port, "invert") == "false")
 	, mCurrentPower(0)
+	, mCurrentPeriod(0x1000)
 {
 	mI2cCommandNumber = ConfigurerHelper::configureInt(configurer, mState, port, "i2cCommandNumber");
 	mState.ready();
@@ -66,7 +67,22 @@ int PowerMotor::power() const
 	return mCurrentPower;
 }
 
+int PowerMotor::period() const
+{
+	return mCurrentPeriod;
+}
+
 void PowerMotor::powerOff()
 {
 	setPower(0);
 }
+
+void PowerMotor::setPeriod(int period)
+{
+	mCurrentPeriod = period;
+	QByteArray command(3, '\0');
+	command[0] = static_cast<char>((mI2cCommandNumber - 4) & 0xFF);
+	command[1] = static_cast<char>(period && 0xFF);
+	command[2] = static_cast<char>(period >> 8);
+	mCommunicator.send(command);
+}

trikControl/src/powerMotor.h

@@ -53,16 +53,24 @@ public slots:
 	/// Returns currently set power of a motor.
 	int power() const;
 
+	/// Returns currently set period of a motor.
+	int period() const;
+
 	/// Turns off motor. This is not the same as setPower(0), because setPower will
 	/// leave motor on in a break mode, and this method will turn motor off.
 	void powerOff();
 
+	/// Sets current motor period.
+	/// Period of pulses is a time interval between two characteristic points of two adjacent pulses. 
+	void setPeriod(int period);
+
 private:
 	I2cCommunicator &mCommunicator;
 	int mI2cCommandNumber;
 	const bool mInvert;
 	int mCurrentPower;
 	DeviceState mState;
+	int mCurrentPeriod;
 };
 
 }

trikControl/system-config.xml

@@ -64,7 +64,7 @@ equal to its class name.
 		<servoMotor period="20000000" invert="false" />
 		<pwmCapture />
 		<powerMotor invert="false" />
-		<analogSensor />
+		<analogSensor rawValue1="0" rawValue2="1024" normalizedValue1="0" normalizedValue2="100" type="Analog" />
 		<encoder />
 		<rangeSensor commonModule="hcsr04" />
 		<digitalSensor />
@@ -163,7 +163,7 @@ equal to its class name.
 	<deviceTypes>
 		<angularServomotor class="servoMotor" min="700000" max="2300000" zero="1500000" stop="0" type="angular" />
 		<continuousRotationServomotor class="servoMotor" min="700000" max="2300000" zero="1500000" stop="0" type="continuousRotation" />
-		<irSensor class="analogSensor" rawValue1="0" rawValue2="1024" normalizedValue1="0" normalizedValue2="100" />
+		<sharpGP2Sensor class="analogSensor" rawValue1="0" rawValue2="1024" normalizedValue1="0" normalizedValue2="100" type="SharpGP2"  values="(25;322)(35;235)(15;511)" />
 		<sonarSensor class="rangeSensor" rawValue1="750" rawValue2="400" normalizedValue1="10" normalizedValue2="20" />
 		<lightSensor class="digitalSensor" min="30000" max="350000" />
 		<volumeSensor class="digitalSensor" min="0" max="100" />