rc2007-soccer/source/Concept/Framework/modules/input/distance_sensor.c

#include "distance_sensor.h"

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

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4);
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
	
	msleep(500);

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

	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4);	
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
	
	uint16 i;
	//Wait for response
	for(i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
	
	//Calculate duration of response
	while((*hardwarePin & pin)&&(result < 300000)) 
	{
		result++;
		asm volatile("nop");
		asm volatile("nop");
		asm volatile("nop");
		asm volatile("nop");
		asm volatile("nop");
		asm volatile("nop");
		asm volatile("nop");
	}

	return (float(result) * DISTANCE_PER_VALUE);
}