rc2007-soccer/source/Concept/Framework/distance_sensor.cpp

#include "distance_sensor.h"

//-----------------------------------------------------------------------------
float Distance_Sensor::GetDistance()
{
	uint32 result = 0;

	//Generate pulse
	*hardwareDDR |= pin;//Set pin output
	*hardwarePort |= pin;//Activate port
	usleep(10);//Wait for 10<31>s
	*hardwarePort &= ~pin;//Deactivate port
	*hardwareDDR &= ~pin;//Set pin input

	//Wait for response
	while(!(*hardwarePin & pin));
	
	//Calculate duration of response
	while(*hardwarePin & pin) 
	{
		result++;
		asm volatile("nop");
	}

	return (float(result) * DISTANCE_PER_VALUE);
}