rover/src/roverapi/rover_sensors.cpp - app4mc/org.eclipse.app4mc.examples - Git at Google

 /*
  * Copyright (c) 2017 FH Dortmund and others
  * All rights reserved. This program and the accompanying materials
  * are made available under the terms of the Eclipse Public License v1.0
  * which accompanies this distribution, and is available at
  * http://www.eclipse.org/legal/epl-v10.html
  *
  * Description:
  *    Rover Sensors API - Interfaces for Rover sensors application development
  *
  * Contributors:
  *    M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
  *    										initial QMC5883L driver implemented 30.11.2017
  *    David Schmelter, Fraunhofer IEM - compass sensor initial implementation
  *    Gael Blondelle - initial API and parameters
  *
  */

 #include <roverapi/basic_psys_rover.h>
 #include <roverapi/rover_sensors.hpp>
 #include <mcp3004.h>
 #include <wiringPiI2C.h>
 #include <math.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <unistd.h>

 static int i2c_hmc588l_fd = -1;

 static int16_t xMinRaw = 0;
 static int16_t xMaxRaw = 0;
 static int16_t yMaxRaw = 0;
 static int16_t yMinRaw = 0;

 rover::RoverSensors::RoverSensors()
 {
 	this->calibration_start = 0;
 }

 void rover::RoverSensors::initialize (void)
 {
 	//wiringPiSetup();

 	this->setupHCSR04UltrasonicSensor(this->ROVER_FRONT);
 	this->setupHCSR04UltrasonicSensor(this->ROVER_REAR);
 	this->setupInfraredSensors();
 	this->setupBearingHMC5883L();
 	this->setupBearingQMC5883L();
 }

 void rover::RoverSensors::setupHCSR04UltrasonicSensor (int sensor_id)
 {
 	int trig_pin, echo_pin;

 	if (sensor_id == this->ROVER_FRONT)
 	{
 		trig_pin = this->TRIG1;
 		echo_pin = this->ECHO1;
 	}
 	else if (sensor_id == this->ROVER_REAR)
 	{
 		trig_pin = this->TRIG0;
 		echo_pin = this->ECHO0;
 	}
 	else
 	{
 		printf ("Invalid Sensor ID for the Ultrasonic Sensor to function setupHCSR04UltrasonicSensor.\n");
 	}

     pinMode(trig_pin, OUTPUT);
     pinMode(echo_pin, INPUT);

     //TRIG pin must start LOW
     digitalWrite(trig_pin, LOW);
     delayMicroseconds(2);
 }

 int rover::RoverSensors::readHCSR04UltrasonicSensor (int sensor_id)
 {
 	int trig_pin, echo_pin;

 	if (sensor_id == this->ROVER_FRONT)
 	{
 		trig_pin = this->TRIG1;
 		echo_pin = this->ECHO1;
 	}
 	else if (sensor_id == this->ROVER_REAR)
 	{
 		trig_pin = this->TRIG0;
 		echo_pin = this->ECHO0;
 	}
 	else
 	{
 		printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readHCSR04UltrasonicSensor.\n");
 	}

 	int distance = 0;
     //Send trig pulse
     digitalWrite(trig_pin, HIGH);
     delayMicroseconds(10);
     digitalWrite(trig_pin, LOW);

     //Wait for echo start
     long startTime = micros();
     while(digitalRead(echo_pin) == LOW && micros() < startTime + 100000);

     //Wait for echo end
     startTime = micros();
     while(digitalRead(echo_pin) == HIGH);
     long travelTime = micros() - startTime;

     //Get distance in cm
     distance = travelTime * 34300;
 	distance = distance / 1000000;
 	distance = distance / 2;
 	// The below protection is to ensure there is no value fluctuation due to timeout
 	if (distance > 40 )
 		distance = 40;

 	//	printf("dist=%d\n",distance);
     return distance;
 }

 void rover::RoverSensors::setupGrooveUltrasonicSensor(void) {
 	//wiringPiSetup();   //Since this can only be used once in a program, we do it in main and comment this.
 }

 int rover::RoverSensors::readGrooveUltrasonicSensor (int sensor_id)
 {
 	int sig_pin;

 	if (sensor_id == this->ROVER_FRONT)
 	{
 		sig_pin = this->SIG1;
 	}
 	else if (sensor_id == this->ROVER_REAR)
 	{
 		sig_pin = this->SIG0;
 	}
 	else
 	{
 		printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readGrooveUltrasonicSensor.\n");
 	}

 	long startTime, stopTime, elapsedTime, distance = 0;
 	pinMode(sig_pin, OUTPUT);
 	digitalWrite(sig_pin, LOW);
 	delayMicroseconds(2);
 	digitalWrite(sig_pin, HIGH);
 	delayMicroseconds(5);
 	digitalWrite(sig_pin, LOW);
 	pinMode(sig_pin,INPUT);

 	startTime = micros();
 	while (digitalRead(sig_pin) == LOW  );
 	startTime = micros();
 	// For values above 40cm, groove sensor is unable to receive signals which causes it to stuck
 	// This is resolved by adding the timeout below.
 	// However, this timeout cause values bigger than 40 to fluctuate
 	while (digitalRead(sig_pin) == HIGH && micros() < startTime + 100000);
 	stopTime = micros();
 	elapsedTime = stopTime - startTime;
 	distance = elapsedTime / 29 /2;
 	// The below protection is to ensure there is no value fluctuation
 	if (distance > 40 )
 		distance = 40;
 	return distance;
 }

 void rover::RoverSensors::setupInfraredSensors (void)
 {
 	/* Init the analog digital converter */
 	mcp3004Setup (BASE, SPI_CHAN); // 3004 and 3008 are the same 4/8 channels
 }

 float rover::RoverSensors::readInfraredSensor (int infrared_sensor_id)
 {
 	float x;
 	float y = analogRead (BASE+infrared_sensor_id);

 	if (infrared_sensor_id != this->ROVER_FRONT_LEFT &&
 			infrared_sensor_id != this->ROVER_FRONT_RIGHT &&
 			infrared_sensor_id != this->ROVER_REAR_LEFT &&
 			infrared_sensor_id != this->ROVER_REAR_RIGHT)
 	{
 		printf ("Invalid Sensor ID for the Infrared Sensor to function readInfraredSensor.\n");
 	}

 	// 1/cm to output voltage is almost linear between
 	// 80cm->0,4V->123
 	// 6cm->3,1V->961
 	// => y=5477*x+55 => x= (y-55)/5477
 	if (y<123){
 		x=100.00;
 	} else {
 		float inverse = (y-55)/5477;
 		//printf("inverse=%f\n",inverse);
 	// x is the distance in cm
 		x = 1/inverse;
 	}

 	return x;
 }

 void rover::RoverSensors::calibrateBearingSensor (void)
 {
 	this->calibration_start = millis();
 }

 void rover::RoverSensors::setupBearingQMC5883L(void)
 {
 	this->setHMC588LAddress(0x0D);
 	this->setHMC588LCalibrationPeriod(10000);
 	this->setHMC588LDeclinationAngle(0.0413);

 	if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0)
 	{
 		printf("Failed to initialize HMC588L compass sensor");
 	}

 	if (i2c_hmc588l_fd >= 0)
 	{
 		wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0B, 0x01); //init SET/PERIOD register

 		/*
 		Define
 		OSR = 512
 		Full Scale Range = 8G(Gauss)
 		ODR = 200HZ
 		set continuous measurement mode
 		*/
 		wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x09, 0x1D);
 	}

 	this->calibration_start = millis();

 	//
 	// To do a software reset
 	// wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0A, 0x80);
 	//

 }

 float rover::RoverSensors::readBearingQMC5883L(void)
 {
 	int8_t buffer[6];

 	//wiringPiI2CWrite (i2c_hmc588l_fd, 0x00); //Start with register 3
 	//delay(25);

 	buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x00); //LSB x
 	buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x01); //MSB x
 	buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x02); //LSB y
 	buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB y
 	buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB z
 	buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05); //MSB z

 	int16_t xRaw = (((int16_t) buffer[1] << 8) & 0xff00) | buffer[0];
 	int16_t yRaw = (((int16_t) buffer[3] << 8) & 0xff00) | buffer[2];
 #ifdef DEBUG
 	printf ("%d %d\n",xRaw,yRaw);
 #endif

 	//if calibration is active calculate minimum and maximum x/y values for calibration
 	if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) {
 		if (xRaw < xMinRaw) {
 			xMinRaw = xRaw;
 		}
 		if (xRaw > xMaxRaw) {
 			xMaxRaw = xRaw;
 		}
 		if (yRaw < yMinRaw) {
 			yMinRaw = yRaw;
 		}
 		if (yRaw > yMaxRaw) {
 			yMaxRaw = yRaw;
 		}
 	}

 	//calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle
 	float xf = xRaw - (float) (xMinRaw + xMaxRaw) / 2.0f;
 	xf = xf / (xMinRaw + xMaxRaw) * 2.0f;

 	//calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle
 	float yf = yRaw - (float) (yMinRaw + yMaxRaw) / 2.0f;
 	yf = yf / (yMinRaw + yMaxRaw) * 2.0f;

 	float bearing = atan2(yf, xf);
 #ifdef DEBUG
 	printf("%f, bearing\n", bearing);
 #endif

 	//location specific magnetic field correction
 	bearing += this->DECLINATION_ANGLE;

 	if (bearing < 0) {
 		bearing += 2 * M_PI;
 	}

 	if (bearing > 2 * M_PI) {
 		bearing -= 2 * M_PI;
 	}

 	float headingDegrees = bearing * (180.0 / M_PI);
 #ifdef DEBUG
 	printf("%lf, headingDegrees\n", headingDegrees);
 #endif
 	return headingDegrees;


 }

 void rover::RoverSensors::setupBearingHMC5883L(void)
 {
 	this->setHMC588LAddress(0x1E);
 	this->setHMC588LCalibrationPeriod(10000);
 	this->setHMC588LDeclinationAngle(0.0413);

 #ifdef DEBUG
 	printf ("HMC588L Address is: %x\n", this->HMC588L_ADDRESS);
 	printf ("HMC588L Calibration period is %d\n", this->CALIBRATION_DURATION);
 	printf ("HMC588L Declination angle is %f\n", this->DECLINATION_ANGLE);
 #endif

 	if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0) {
 		printf("Failed to initialize HMC588L compass sensor");
 	}

 	if (i2c_hmc588l_fd >= 0) {
 		int8_t gain = 5;

 		wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x00, 0x70); // 8-average, 15 Hz default, positive self test measurement
 		wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x01, gain << 5); // Gain
 		wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x02, 0x00); // Continuous-measurement mode
 	}

 	this->calibration_start = millis();
 }

 float rover::RoverSensors::readBearingHMC5883L(void)
 {
 	int8_t buffer[6];

 	//potential optimization: wiringPiI2CReadReg16
 	buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB x
 	buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB x
 	buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05); //MSB z
 	buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x06); //LSB z
 	buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x07); //MSB y
 	buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x08); //LSB y

 	int16_t xRaw = (((int16_t) buffer[0] << 8) & 0xff00) | buffer[1];
 	//int16_t zRaw = (((int16_t) buffer[2] << 8) & 0xff00) | buffer[3];
 	int16_t yRaw = (((int16_t) buffer[4] << 8) & 0xff00) | buffer[5];

 	//if calibration is active calculate minimum and maximum x/y values for calibration
 	if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) {
 		if (xRaw < xMinRaw) {
 			xMinRaw = xRaw;
 		}
 		if (xRaw > xMaxRaw) {
 			xMaxRaw = xRaw;
 		}
 		if (yRaw < yMinRaw) {
 			yMinRaw = yRaw;
 		}
 		if (yRaw > yMaxRaw) {
 			yMaxRaw = yRaw;
 		}
 	}

 	//calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle
 	float xf = xRaw - (float) (xMinRaw + xMaxRaw) / 2.0f;
 	xf = xf / (xMinRaw + xMaxRaw) * 2.0f;

 	//calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle
 	float yf = yRaw - (float) (yMinRaw + yMaxRaw) / 2.0f;
 	yf = yf / (yMinRaw + yMaxRaw) * 2.0f;

 	float bearing = atan2(yf, xf);
 #ifdef DEBUG
 	printf("%f, bearing\n", bearing);
 #endif

 	//location specific magnetic field correction
 	bearing += this->DECLINATION_ANGLE;

 	if (bearing < 0) {
 		bearing += 2 * M_PI;
 	}

 	if (bearing > 2 * M_PI) {
 		bearing -= 2 * M_PI;
 	}

 	float headingDegrees = bearing * (180.0 / M_PI);
 #ifdef DEBUG
 	printf("%lf, headingDegrees\n", headingDegrees);
 #endif
 	return headingDegrees;
 }

 float rover::RoverSensors::readTemperature (void)
 {
 	int data[5] = { 0, 0, 0, 0, 0 };

 	uint8_t laststate;
 	uint8_t counter;
 	uint8_t j;
 	uint8_t i;

 	int try_again = 1;
 	float f, h, c;

 	while (try_again == 1)
 	{
 		data[0] = data[1] = data[2] = data[3] = data[4] = 0;
 		laststate = HIGH;
 		counter = 0;
 		j = 0;

 		/* pull pin down for 18 milliseconds */
 		pinMode( this->DHT22_RPI_PIN, OUTPUT );
 		digitalWrite( this->DHT22_RPI_PIN, LOW );
 		delay( 18 );

 		/* prepare to read the pin */
 		pinMode( this->DHT22_RPI_PIN, INPUT );

 		/* detect change and read data */
 		for ( i = 0; i < this->MAX_TIMINGS; i++ )
 		{
 			counter = 0;
 			while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
 			{
 				counter++;
 				delayMicroseconds( 1 );
 				if ( counter == 255 )
 				{
 					break;
 				}
 			}
 			laststate = digitalRead( this->DHT22_RPI_PIN );

 			if ( counter == 255 )
 				break;

 			/* ignore first 3 transitions */
 			if ( (i >= 4) && (i % 2 == 0) )
 			{
 				/* shove each bit into the storage bytes */
 				data[j / 8] <<= 1;
 				if ( counter > 16 )
 					data[j / 8] |= 1;
 				j++;
 			}
 		}

 		/*
 		 * check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
 		 * print it out if data is good
 		 */
 		if ( (j >= 40) &&
 			 (data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
 		{
 			h = (float)((data[0] << 8) + data[1]) / 10;
 			if ( h > 100 )
 			{
 				h = data[0];	// for DHT11
 			}
 			c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
 			if ( c > 125 )
 			{
 				c = data[2];	// for DHT11
 			}
 			if ( data[2] & 0x80 )
 			{
 				c = -c;
 			}
 			f = c * 1.8f + 32;
 #ifdef DEBUG
 			printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
 #endif
 			try_again = 0;
 		}
 		else
 		{
 			/* Data not good */
 			try_again = 1;
 			//printf ("Data not good, skipping\n");

 		}
 	}

 	/* Return temperature */
 	return c;
 }

 float rover::RoverSensors::readHumidity (void)
 {
 	int data[5] = { 0, 0, 0, 0, 0 };

 	uint8_t laststate;
 	uint8_t counter;
 	uint8_t j;
 	uint8_t i;

 	int try_again = 1;
 	float f, h, c;

 	while (try_again == 1)
 	{
 		data[0] = data[1] = data[2] = data[3] = data[4] = 0;
 		laststate = HIGH;
 		counter = 0;
 		j = 0;

 		/* pull pin down for 18 milliseconds */
 		pinMode( this->DHT22_RPI_PIN, OUTPUT );
 		digitalWrite( this->DHT22_RPI_PIN, LOW );
 		delay( 18 );

 		/* prepare to read the pin */
 		pinMode( this->DHT22_RPI_PIN, INPUT );

 		/* detect change and read data */
 		for ( i = 0; i < this->MAX_TIMINGS; i++ )
 		{
 			counter = 0;
 			while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
 			{
 				counter++;
 				delayMicroseconds( 1 );
 				if ( counter == 255 )
 				{
 					break;
 				}
 			}
 			laststate = digitalRead( this->DHT22_RPI_PIN );

 			if ( counter == 255 )
 				break;

 			/* ignore first 3 transitions */
 			if ( (i >= 4) && (i % 2 == 0) )
 			{
 				/* shove each bit into the storage bytes */
 				data[j / 8] <<= 1;
 				if ( counter > 16 )
 					data[j / 8] |= 1;
 				j++;
 			}
 		}

 		/*
 		 * check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
 		 * print it out if data is good
 		 */
 		if ( (j >= 40) &&
 			 (data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
 		{
 			h = (float)((data[0] << 8) + data[1]) / 10;
 			if ( h > 100 )
 			{
 				h = data[0];	// for DHT11
 			}
 			c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
 			if ( c > 125 )
 			{
 				c = data[2];	// for DHT11
 			}
 			if ( data[2] & 0x80 )
 			{
 				c = -c;
 			}
 			f = c * 1.8f + 32;
 #ifdef DEBUG
 			printf( "Humidity = %.1f %% Temperature = %.1f *C (%.1f *F)\n", h, c, f );
 #endif
 			try_again = 0;
 		}
 		else
 		{
 			/* Data not good */
 			try_again = 1;
 			//printf ("Data not good, skipping\n");
 		}
 	}

 	/* Return humidity */
 	return h;
 }

 void rover::RoverSensors::setHMC588LAddress (int address)
 {
 	this->HMC588L_ADDRESS = address;
 }

 void rover::RoverSensors::setHMC588LDeclinationAngle (float angle)
 {
 	this->DECLINATION_ANGLE = angle;
 }

 void rover::RoverSensors::setHMC588LCalibrationPeriod(int period)
 {
 	this->CALIBRATION_DURATION = period;
 }
	/*
	* Copyright (c) 2017 FH Dortmund and others
	* All rights reserved. This program and the accompanying materials
	* are made available under the terms of the Eclipse Public License v1.0
	* which accompanies this distribution, and is available at
	* http://www.eclipse.org/legal/epl-v10.html
	*
	* Description:
	* Rover Sensors API - Interfaces for Rover sensors application development
	*
	* Contributors:
	* M.Ozcelikors <mozcelikors@gmail.com>, created C++ API 17.11.2017
	* initial QMC5883L driver implemented 30.11.2017
	* David Schmelter, Fraunhofer IEM - compass sensor initial implementation
	* Gael Blondelle - initial API and parameters
	*
	*/

	#include <roverapi/basic_psys_rover.h>
	#include <roverapi/rover_sensors.hpp>
	#include <mcp3004.h>
	#include <wiringPiI2C.h>
	#include <math.h>
	#include <stdint.h>
	#include <stdio.h>
	#include <unistd.h>

	static int i2c_hmc588l_fd = -1;

	static int16_t xMinRaw = 0;
	static int16_t xMaxRaw = 0;
	static int16_t yMaxRaw = 0;
	static int16_t yMinRaw = 0;

	rover::RoverSensors::RoverSensors()
	{
	this->calibration_start = 0;
	}

	void rover::RoverSensors::initialize (void)
	{
	//wiringPiSetup();

	this->setupHCSR04UltrasonicSensor(this->ROVER_FRONT);
	this->setupHCSR04UltrasonicSensor(this->ROVER_REAR);
	this->setupInfraredSensors();
	this->setupBearingHMC5883L();
	this->setupBearingQMC5883L();
	}

	void rover::RoverSensors::setupHCSR04UltrasonicSensor (int sensor_id)
	{
	int trig_pin, echo_pin;

	if (sensor_id == this->ROVER_FRONT)
	{
	trig_pin = this->TRIG1;
	echo_pin = this->ECHO1;
	}
	else if (sensor_id == this->ROVER_REAR)
	{
	trig_pin = this->TRIG0;
	echo_pin = this->ECHO0;
	}
	else
	{
	printf ("Invalid Sensor ID for the Ultrasonic Sensor to function setupHCSR04UltrasonicSensor.\n");
	}

	pinMode(trig_pin, OUTPUT);
	pinMode(echo_pin, INPUT);

	//TRIG pin must start LOW
	digitalWrite(trig_pin, LOW);
	delayMicroseconds(2);
	}

	int rover::RoverSensors::readHCSR04UltrasonicSensor (int sensor_id)
	{
	int trig_pin, echo_pin;

	if (sensor_id == this->ROVER_FRONT)
	{
	trig_pin = this->TRIG1;
	echo_pin = this->ECHO1;
	}
	else if (sensor_id == this->ROVER_REAR)
	{
	trig_pin = this->TRIG0;
	echo_pin = this->ECHO0;
	}
	else
	{
	printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readHCSR04UltrasonicSensor.\n");
	}

	int distance = 0;
	//Send trig pulse
	digitalWrite(trig_pin, HIGH);
	delayMicroseconds(10);
	digitalWrite(trig_pin, LOW);

	//Wait for echo start
	long startTime = micros();
	while(digitalRead(echo_pin) == LOW && micros() < startTime + 100000);

	//Wait for echo end
	startTime = micros();
	while(digitalRead(echo_pin) == HIGH);
	long travelTime = micros() - startTime;

	//Get distance in cm
	distance = travelTime * 34300;
	distance = distance / 1000000;
	distance = distance / 2;
	// The below protection is to ensure there is no value fluctuation due to timeout
	if (distance > 40 )
	distance = 40;

	// printf("dist=%d\n",distance);
	return distance;
	}

	void rover::RoverSensors::setupGrooveUltrasonicSensor(void) {
	//wiringPiSetup(); //Since this can only be used once in a program, we do it in main and comment this.
	}

	int rover::RoverSensors::readGrooveUltrasonicSensor (int sensor_id)
	{
	int sig_pin;

	if (sensor_id == this->ROVER_FRONT)
	{
	sig_pin = this->SIG1;
	}
	else if (sensor_id == this->ROVER_REAR)
	{
	sig_pin = this->SIG0;
	}
	else
	{
	printf ("Invalid Sensor ID for the Ultrasonic Sensor to function readGrooveUltrasonicSensor.\n");
	}

	long startTime, stopTime, elapsedTime, distance = 0;
	pinMode(sig_pin, OUTPUT);
	digitalWrite(sig_pin, LOW);
	delayMicroseconds(2);
	digitalWrite(sig_pin, HIGH);
	delayMicroseconds(5);
	digitalWrite(sig_pin, LOW);
	pinMode(sig_pin,INPUT);

	startTime = micros();
	while (digitalRead(sig_pin) == LOW );
	startTime = micros();
	// For values above 40cm, groove sensor is unable to receive signals which causes it to stuck
	// This is resolved by adding the timeout below.
	// However, this timeout cause values bigger than 40 to fluctuate
	while (digitalRead(sig_pin) == HIGH && micros() < startTime + 100000);
	stopTime = micros();
	elapsedTime = stopTime - startTime;
	distance = elapsedTime / 29 /2;
	// The below protection is to ensure there is no value fluctuation
	if (distance > 40 )
	distance = 40;
	return distance;
	}

	void rover::RoverSensors::setupInfraredSensors (void)
	{
	/* Init the analog digital converter */
	mcp3004Setup (BASE, SPI_CHAN); // 3004 and 3008 are the same 4/8 channels
	}

	float rover::RoverSensors::readInfraredSensor (int infrared_sensor_id)
	{
	float x;
	float y = analogRead (BASE+infrared_sensor_id);

	if (infrared_sensor_id != this->ROVER_FRONT_LEFT &&
	infrared_sensor_id != this->ROVER_FRONT_RIGHT &&
	infrared_sensor_id != this->ROVER_REAR_LEFT &&
	infrared_sensor_id != this->ROVER_REAR_RIGHT)
	{
	printf ("Invalid Sensor ID for the Infrared Sensor to function readInfraredSensor.\n");
	}

	// 1/cm to output voltage is almost linear between
	// 80cm->0,4V->123
	// 6cm->3,1V->961
	// => y=5477*x+55 => x= (y-55)/5477
	if (y<123){
	x=100.00;
	} else {
	float inverse = (y-55)/5477;
	//printf("inverse=%f\n",inverse);
	// x is the distance in cm
	x = 1/inverse;
	}

	return x;
	}

	void rover::RoverSensors::calibrateBearingSensor (void)
	{
	this->calibration_start = millis();
	}

	void rover::RoverSensors::setupBearingQMC5883L(void)
	{
	this->setHMC588LAddress(0x0D);
	this->setHMC588LCalibrationPeriod(10000);
	this->setHMC588LDeclinationAngle(0.0413);

	if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0)
	{
	printf("Failed to initialize HMC588L compass sensor");
	}

	if (i2c_hmc588l_fd >= 0)
	{
	wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0B, 0x01); //init SET/PERIOD register

	/*
	Define
	OSR = 512
	Full Scale Range = 8G(Gauss)
	ODR = 200HZ
	set continuous measurement mode
	*/
	wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x09, 0x1D);
	}

	this->calibration_start = millis();

	//
	// To do a software reset
	// wiringPiI2CWriteReg8 (i2c_hmc588l_fd, 0x0A, 0x80);
	//

	}

	float rover::RoverSensors::readBearingQMC5883L(void)
	{
	int8_t buffer[6];

	//wiringPiI2CWrite (i2c_hmc588l_fd, 0x00); //Start with register 3
	//delay(25);

	buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x00); //LSB x
	buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x01); //MSB x
	buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x02); //LSB y
	buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB y
	buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB z
	buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05); //MSB z

	int16_t xRaw = (((int16_t) buffer[1] << 8) & 0xff00) \| buffer[0];
	int16_t yRaw = (((int16_t) buffer[3] << 8) & 0xff00) \| buffer[2];
	#ifdef DEBUG
	printf ("%d %d\n",xRaw,yRaw);
	#endif

	//if calibration is active calculate minimum and maximum x/y values for calibration
	if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) {
	if (xRaw < xMinRaw) {
	xMinRaw = xRaw;
	}
	if (xRaw > xMaxRaw) {
	xMaxRaw = xRaw;
	}
	if (yRaw < yMinRaw) {
	yMinRaw = yRaw;
	}
	if (yRaw > yMaxRaw) {
	yMaxRaw = yRaw;
	}
	}

	//calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle
	float xf = xRaw - (float) (xMinRaw + xMaxRaw) / 2.0f;
	xf = xf / (xMinRaw + xMaxRaw) * 2.0f;

	//calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle
	float yf = yRaw - (float) (yMinRaw + yMaxRaw) / 2.0f;
	yf = yf / (yMinRaw + yMaxRaw) * 2.0f;

	float bearing = atan2(yf, xf);
	#ifdef DEBUG
	printf("%f, bearing\n", bearing);
	#endif

	//location specific magnetic field correction
	bearing += this->DECLINATION_ANGLE;

	if (bearing < 0) {
	bearing += 2 * M_PI;
	}

	if (bearing > 2 * M_PI) {
	bearing -= 2 * M_PI;
	}

	float headingDegrees = bearing * (180.0 / M_PI);
	#ifdef DEBUG
	printf("%lf, headingDegrees\n", headingDegrees);
	#endif
	return headingDegrees;


	}

	void rover::RoverSensors::setupBearingHMC5883L(void)
	{
	this->setHMC588LAddress(0x1E);
	this->setHMC588LCalibrationPeriod(10000);
	this->setHMC588LDeclinationAngle(0.0413);

	#ifdef DEBUG
	printf ("HMC588L Address is: %x\n", this->HMC588L_ADDRESS);
	printf ("HMC588L Calibration period is %d\n", this->CALIBRATION_DURATION);
	printf ("HMC588L Declination angle is %f\n", this->DECLINATION_ANGLE);
	#endif

	if ((i2c_hmc588l_fd = wiringPiI2CSetup(this->HMC588L_ADDRESS)) < 0) {
	printf("Failed to initialize HMC588L compass sensor");
	}

	if (i2c_hmc588l_fd >= 0) {
	int8_t gain = 5;

	wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x00, 0x70); // 8-average, 15 Hz default, positive self test measurement
	wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x01, gain << 5); // Gain
	wiringPiI2CWriteReg8(i2c_hmc588l_fd, 0x02, 0x00); // Continuous-measurement mode
	}

	this->calibration_start = millis();
	}

	float rover::RoverSensors::readBearingHMC5883L(void)
	{
	int8_t buffer[6];

	//potential optimization: wiringPiI2CReadReg16
	buffer[0] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x03); //MSB x
	buffer[1] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x04); //LSB x
	buffer[2] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x05); //MSB z
	buffer[3] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x06); //LSB z
	buffer[4] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x07); //MSB y
	buffer[5] = wiringPiI2CReadReg8(i2c_hmc588l_fd, 0x08); //LSB y

	int16_t xRaw = (((int16_t) buffer[0] << 8) & 0xff00) \| buffer[1];
	//int16_t zRaw = (((int16_t) buffer[2] << 8) & 0xff00) \| buffer[3];
	int16_t yRaw = (((int16_t) buffer[4] << 8) & 0xff00) \| buffer[5];

	//if calibration is active calculate minimum and maximum x/y values for calibration
	if (millis() <= this->calibration_start + this->CALIBRATION_DURATION) {
	if (xRaw < xMinRaw) {
	xMinRaw = xRaw;
	}
	if (xRaw > xMaxRaw) {
	xMaxRaw = xRaw;
	}
	if (yRaw < yMinRaw) {
	yMinRaw = yRaw;
	}
	if (yRaw > yMaxRaw) {
	yMaxRaw = yRaw;
	}
	}

	//calibration: move and scale x coordinates based on minimum and maximum values to get a unit circle
	float xf = xRaw - (float) (xMinRaw + xMaxRaw) / 2.0f;
	xf = xf / (xMinRaw + xMaxRaw) * 2.0f;

	//calibration: move and scale y coordinates based on minimum and maximum values to get a unit circle
	float yf = yRaw - (float) (yMinRaw + yMaxRaw) / 2.0f;
	yf = yf / (yMinRaw + yMaxRaw) * 2.0f;

	float bearing = atan2(yf, xf);
	#ifdef DEBUG
	printf("%f, bearing\n", bearing);
	#endif

	//location specific magnetic field correction
	bearing += this->DECLINATION_ANGLE;

	if (bearing < 0) {
	bearing += 2 * M_PI;
	}

	if (bearing > 2 * M_PI) {
	bearing -= 2 * M_PI;
	}

	float headingDegrees = bearing * (180.0 / M_PI);
	#ifdef DEBUG
	printf("%lf, headingDegrees\n", headingDegrees);
	#endif
	return headingDegrees;
	}

	float rover::RoverSensors::readTemperature (void)
	{
	int data[5] = { 0, 0, 0, 0, 0 };

	uint8_t laststate;
	uint8_t counter;
	uint8_t j;
	uint8_t i;

	int try_again = 1;
	float f, h, c;

	while (try_again == 1)
	{
	data[0] = data[1] = data[2] = data[3] = data[4] = 0;
	laststate = HIGH;
	counter = 0;
	j = 0;

	/* pull pin down for 18 milliseconds */
	pinMode( this->DHT22_RPI_PIN, OUTPUT );
	digitalWrite( this->DHT22_RPI_PIN, LOW );
	delay( 18 );

	/* prepare to read the pin */
	pinMode( this->DHT22_RPI_PIN, INPUT );

	/* detect change and read data */
	for ( i = 0; i < this->MAX_TIMINGS; i++ )
	{
	counter = 0;
	while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
	{
	counter++;
	delayMicroseconds( 1 );
	if ( counter == 255 )
	{
	break;
	}
	}
	laststate = digitalRead( this->DHT22_RPI_PIN );

	if ( counter == 255 )
	break;

	/* ignore first 3 transitions */
	if ( (i >= 4) && (i % 2 == 0) )
	{
	/* shove each bit into the storage bytes */
	data[j / 8] <<= 1;
	if ( counter > 16 )
	data[j / 8] \|= 1;
	j++;
	}
	}

	/*
	* check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
	* print it out if data is good
	*/
	if ( (j >= 40) &&
	(data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
	{
	h = (float)((data[0] << 8) + data[1]) / 10;
	if ( h > 100 )
	{
	h = data[0]; // for DHT11
	}
	c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
	if ( c > 125 )
	{
	c = data[2]; // for DHT11
	}
	if ( data[2] & 0x80 )
	{
	c = -c;
	}
	f = c * 1.8f + 32;
	#ifdef DEBUG
	printf( "Humidity = %.1f %% Temperature = %.1f C (%.1f F)\n", h, c, f );
	#endif
	try_again = 0;
	}
	else
	{
	/* Data not good */
	try_again = 1;
	//printf ("Data not good, skipping\n");

	}
	}

	/* Return temperature */
	return c;
	}

	float rover::RoverSensors::readHumidity (void)
	{
	int data[5] = { 0, 0, 0, 0, 0 };

	uint8_t laststate;
	uint8_t counter;
	uint8_t j;
	uint8_t i;

	int try_again = 1;
	float f, h, c;

	while (try_again == 1)
	{
	data[0] = data[1] = data[2] = data[3] = data[4] = 0;
	laststate = HIGH;
	counter = 0;
	j = 0;

	/* pull pin down for 18 milliseconds */
	pinMode( this->DHT22_RPI_PIN, OUTPUT );
	digitalWrite( this->DHT22_RPI_PIN, LOW );
	delay( 18 );

	/* prepare to read the pin */
	pinMode( this->DHT22_RPI_PIN, INPUT );

	/* detect change and read data */
	for ( i = 0; i < this->MAX_TIMINGS; i++ )
	{
	counter = 0;
	while ( digitalRead( this->DHT22_RPI_PIN ) == laststate )
	{
	counter++;
	delayMicroseconds( 1 );
	if ( counter == 255 )
	{
	break;
	}
	}
	laststate = digitalRead( this->DHT22_RPI_PIN );

	if ( counter == 255 )
	break;

	/* ignore first 3 transitions */
	if ( (i >= 4) && (i % 2 == 0) )
	{
	/* shove each bit into the storage bytes */
	data[j / 8] <<= 1;
	if ( counter > 16 )
	data[j / 8] \|= 1;
	j++;
	}
	}

	/*
	* check we read 40 bits (8bit x 5 ) + verify checksum in the last byte
	* print it out if data is good
	*/
	if ( (j >= 40) &&
	(data[4] == ( (data[0] + data[1] + data[2] + data[3]) & 0xFF) ) )
	{
	h = (float)((data[0] << 8) + data[1]) / 10;
	if ( h > 100 )
	{
	h = data[0]; // for DHT11
	}
	c = (float)(((data[2] & 0x7F) << 8) + data[3]) / 10;
	if ( c > 125 )
	{
	c = data[2]; // for DHT11
	}
	if ( data[2] & 0x80 )
	{
	c = -c;
	}
	f = c * 1.8f + 32;
	#ifdef DEBUG
	printf( "Humidity = %.1f %% Temperature = %.1f C (%.1f F)\n", h, c, f );
	#endif
	try_again = 0;
	}
	else
	{
	/* Data not good */
	try_again = 1;
	//printf ("Data not good, skipping\n");
	}
	}

	/* Return humidity */
	return h;
	}

	void rover::RoverSensors::setHMC588LAddress (int address)
	{
	this->HMC588L_ADDRESS = address;
	}

	void rover::RoverSensors::setHMC588LDeclinationAngle (float angle)
	{
	this->DECLINATION_ANGLE = angle;
	}

	void rover::RoverSensors::setHMC588LCalibrationPeriod(int period)
	{
	this->CALIBRATION_DURATION = period;
	}